import javax.vecmath.Matrix3f; //导入依赖的package包/类
public void fillEllipsoid(Point3f center, Point3f[] points, int x, int y,
                            int z, int diameter, Matrix3f mToEllipsoidal,
                            double[] coef, Matrix4f mDeriv,
                            int selectedOctant, Point3i[] octantPoints) {
  switch (diameter) {
  case 1:
    plotPixelClipped(argbCurrent, x, y, z);
    return;
  case 0:
    return;
  }
  if (diameter <= (antialiasThisFrame ? Sphere3D.maxSphereDiameter2
      : Sphere3D.maxSphereDiameter))
    sphere3d.render(shadesCurrent, !addAllPixels, diameter, x, y, z,
        mToEllipsoidal, coef, mDeriv, selectedOctant, octantPoints);
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public void setNotionalUnitCell(float[] info, Matrix3f matUnitCellOrientation, Point3f unitCellOffset) {
  notionalUnitCell = new float[info.length];
  this.unitCellOffset = unitCellOffset;
  for (int i = 0; i < info.length; i++)
    notionalUnitCell[i] = info[i];
  haveUnitCell = true;
  setAtomSetAuxiliaryInfo("notionalUnitcell", notionalUnitCell);
  setGlobalBoolean(GLOBAL_UNITCELLS);
  getSymmetry().setUnitCell(notionalUnitCell);
  // we need to set the auxiliary info as well, because 
  // ModelLoader creates a new symmetry object.
  if (unitCellOffset != null){
    symmetry.setUnitCellOffset(unitCellOffset);
    setAtomSetAuxiliaryInfo("unitCellOffset", unitCellOffset);
  }
  if (matUnitCellOrientation != null) {
    symmetry.setUnitCellOrientation(matUnitCellOrientation);
    setAtomSetAuxiliaryInfo("matUnitCellOrientation", matUnitCellOrientation);
  }
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public void setSymmetryInfo(int modelIndex, Map<String, Object> modelAuxiliaryInfo) {
  symmetryInfo = new SymmetryInfo();
  float[] notionalUnitcell = symmetryInfo.setSymmetryInfo(modelIndex,
      modelAuxiliaryInfo);
  if (notionalUnitcell == null)
    return;
  setUnitCell(notionalUnitcell);
  modelAuxiliaryInfo.put("infoUnitCell", getUnitCellAsArray());
  setUnitCellOffset((Point3f) modelAuxiliaryInfo.get("unitCellOffset"));
  if (modelAuxiliaryInfo.containsKey("jmolData"))
    setUnitCellAllFractionalRelative(true);
  Matrix3f matUnitCellOrientation = (Matrix3f) modelAuxiliaryInfo.get("matUnitCellOrientation");
  if (matUnitCellOrientation != null)
    setUnitCellOrientation(matUnitCellOrientation);
  if (Logger.debugging)
    Logger
        .debug("symmetryInfos[" + modelIndex + "]:\n" + unitCell.dumpInfo(true));
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public static Object unescapeMatrix(String strMatrix) {
  if (strMatrix == null || strMatrix.length() == 0)
    return strMatrix;
  String str = strMatrix.replace('\n', ' ').trim();
  if (str.lastIndexOf("[[") != 0 || str.indexOf("]]") != str.length() - 2)
    return strMatrix;
  float[] points = new float[16];
  str = str.substring(2, str.length() - 2).replace('[',' ').replace(']',' ').replace(',',' ');
  int[] next = new int[1];
  int nPoints = 0;
  for (; nPoints < 16; nPoints++) {
    points[nPoints] = Parser.parseFloat(str, next);
    if (Float.isNaN(points[nPoints])) {
      break;
    }
  }
  if (!Float.isNaN(Parser.parseFloat(str, next)))
    return strMatrix; // overflow
  if (nPoints == 9)
    return new Matrix3f(points);
  if (nPoints == 16)
    return new Matrix4f(points);
  return strMatrix;
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public static Matrix3f setEllipsoidMatrix(Vector3f[] unitAxes, float[] lengths, Vector3f vTemp, Matrix3f mat) {
  /*
   * Create a matrix that transforms cartesian coordinates
   * into ellipsoidal coordinates, where in that system we 
   * are drawing a sphere. 
   *
   */
  
  for (int i = 0; i < 3; i++) {
    vTemp.set(unitAxes[i]);
    vTemp.scale(lengths[i]);
    mat.setColumn(i, vTemp);
  }
  mat.invert(mat);
  return mat;
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
protected void setSphereMatrix(Point3f center, float rx, float ry, float rz,
                               AxisAngle4f a, Matrix4f sphereMatrix) {
  if (a != null) {
    Matrix3f mq = new Matrix3f();
    Matrix3f m = new Matrix3f();
    m.m00 = rx;
    m.m11 = ry;
    m.m22 = rz;
    mq.set(a);
    mq.mul(m);
    sphereMatrix.set(mq);
  } else {
    sphereMatrix.setIdentity();
    sphereMatrix.m00 = rx;
    sphereMatrix.m11 = ry;
    sphereMatrix.m22 = rz;
  }
  sphereMatrix.m03 = center.x;
  sphereMatrix.m13 = center.y;
  sphereMatrix.m23 = center.z;
  sphereMatrix.m33 = 1;
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public void init(/*StackAlloc psa,*/
		Matrix3f wrot0, Vector3f pos0, ConvexShape shape0,
		Matrix3f wrot1, Vector3f pos1, ConvexShape shape1,
		float pmargin) {
	pushStack();
	wrotations[0].set(wrot0);
	positions[0].set(pos0);
	shapes[0] = shape0;
	wrotations[1].set(wrot1);
	positions[1].set(pos1);
	shapes[1] = shape1;
	//sa		=psa;
	//sablock	=sa->beginBlock();
	margin = pmargin;
	failed = false;
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
/**
 * Sets rotation matrix from euler angles. The euler angles are applied in ZYX
 * order. This means a vector is first rotated about X then Y and then Z axis.
 */
public static void setEulerZYX(Matrix3f mat, float eulerX, float eulerY, float eulerZ) {
	float ci = (float) Math.cos(eulerX);
	float cj = (float) Math.cos(eulerY);
	float ch = (float) Math.cos(eulerZ);
	float si = (float) Math.sin(eulerX);
	float sj = (float) Math.sin(eulerY);
	float sh = (float) Math.sin(eulerZ);
	float cc = ci * ch;
	float cs = ci * sh;
	float sc = si * ch;
	float ss = si * sh;

	mat.setRow(0, cj * ch, sj * sc - cs, sj * cc + ss);
	mat.setRow(1, cj * sh, sj * ss + cc, sj * cs - sc);
	mat.setRow(2, -sj, cj * si, cj * ci);
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public static void setRotation(Matrix3f dest, Quat4f q) {
	float d = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
	assert (d != 0f);
	float s = 2f / d;
	float xs = q.x * s, ys = q.y * s, zs = q.z * s;
	float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
	float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
	float yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
	dest.m00 = 1f - (yy + zz);
	dest.m01 = xy - wz;
	dest.m02 = xz + wy;
	dest.m10 = xy + wz;
	dest.m11 = 1f - (xx + zz);
	dest.m12 = yz - wx;
	dest.m20 = xz - wy;
	dest.m21 = yz + wx;
	dest.m22 = 1f - (xx + yy);
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public static void transformAabb(Vector3f halfExtents, float margin, Transform t, Vector3f aabbMinOut, Vector3f aabbMaxOut) {
	Vector3f halfExtentsWithMargin = Stack.alloc(Vector3f.class);
	halfExtentsWithMargin.x = halfExtents.x + margin;
	halfExtentsWithMargin.y = halfExtents.y + margin;
	halfExtentsWithMargin.z = halfExtents.z + margin;

	Matrix3f abs_b = Stack.alloc(t.basis);
	MatrixUtil.absolute(abs_b);

	Vector3f tmp = Stack.alloc(Vector3f.class);

	Vector3f center = Stack.alloc(t.origin);
	Vector3f extent = Stack.alloc(Vector3f.class);
	abs_b.getRow(0, tmp);
	extent.x = tmp.dot(halfExtentsWithMargin);
	abs_b.getRow(1, tmp);
	extent.y = tmp.dot(halfExtentsWithMargin);
	abs_b.getRow(2, tmp);
	extent.z = tmp.dot(halfExtentsWithMargin);

	aabbMinOut.sub(center, extent);
	aabbMaxOut.add(center, extent);
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
/**
 * Constraint between two different rigidbodies.
 */
public void init(Matrix3f world2A,
		Matrix3f world2B,
		Vector3f rel_pos1, Vector3f rel_pos2,
		Vector3f jointAxis,
		Vector3f inertiaInvA,
		float massInvA,
		Vector3f inertiaInvB,
		float massInvB)
{
	linearJointAxis.set(jointAxis);

	aJ.cross(rel_pos1, linearJointAxis);
	world2A.transform(aJ);

	bJ.set(linearJointAxis);
	bJ.negate();
	bJ.cross(rel_pos2, bJ);
	world2B.transform(bJ);

	VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ);
	VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ);
	Adiag = massInvA + m_0MinvJt.dot(aJ) + massInvB + m_1MinvJt.dot(bJ);

	assert (Adiag > 0f);
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
protected void buildLinearJacobian(/*JacobianEntry jacLinear*/int jacLinear_index, Vector3f normalWorld, Vector3f pivotAInW, Vector3f pivotBInW) {
	Matrix3f mat1 = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
	mat1.transpose();

	Matrix3f mat2 = rbB.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
	mat2.transpose();

	Vector3f tmpVec = Stack.alloc(Vector3f.class);
	
	Vector3f tmp1 = Stack.alloc(Vector3f.class);
	tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition(tmpVec));

	Vector3f tmp2 = Stack.alloc(Vector3f.class);
	tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition(tmpVec));

	jacLinear[jacLinear_index].init(
			mat1,
			mat2,
			tmp1,
			tmp2,
			normalWorld,
			rbA.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
			rbA.getInvMass(),
			rbB.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
			rbB.getInvMass());
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
/**
 * Returns <code>true</code> if the rotation matrix matches only rotations of 
 * a multiple of 90� degrees around x, y or z axis.
 */
private boolean isOrthogonalRotation(Transform3D transformation)
{
	Matrix3f matrix = new Matrix3f();
	transformation.get(matrix);
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			// Return false if the matrix contains a value different from 0 1 or -1
			if (Math.abs(matrix.getElement(i, j)) > 1E-6 && Math.abs(matrix.getElement(i, j) - 1) > 1E-6
					&& Math.abs(matrix.getElement(i, j) + 1) > 1E-6)
			{
				return false;
			}
		}
	}
	return true;
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
@Override
public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
	Vector3f halfExtents = getHalfExtentsWithoutMargin(Stack.alloc(Vector3f.class));

	Matrix3f abs_b = Stack.alloc(t.basis);
	MatrixUtil.absolute(abs_b);

	Vector3f tmp = Stack.alloc(Vector3f.class);

	Vector3f center = Stack.alloc(t.origin);
	Vector3f extent = Stack.alloc(Vector3f.class);
	abs_b.getRow(0, tmp);
	extent.x = tmp.dot(halfExtents);
	abs_b.getRow(1, tmp);
	extent.y = tmp.dot(halfExtents);
	abs_b.getRow(2, tmp);
	extent.z = tmp.dot(halfExtents);

	Vector3f margin = Stack.alloc(Vector3f.class);
	margin.set(getMargin(), getMargin(), getMargin());
	extent.add(margin);

	aabbMin.sub(center, extent);
	aabbMax.add(center, extent);
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
private void updateConeTransforms(ProGAL.geom3d.volumes.Cone c){

		Transform3D trans = new Transform3D();
		Vector v1 = new Vector(0,-1,0);
		Vector v2 = c.getAxis();//c.p1.vectorTo(c.p2);
		if(v2.length()>0.000001 && v1.angle(v2)>0.00001)
		{ 
			//Matrix m = Matrix.createRotationMatrix(v1.angle(v2), v1.cross(v2).normIn());
			//trans.set(m.getCoordArray());
			Vector v = v1.cross(v2).normalizeThis();
			Matrix m4 = Matrix.createRotationMatrix(v1.angle(v2), v);
//			trans.set(to4x4CoordArray(m4));
			trans.setRotation(new Matrix3f(to3x3CoordArray(m4)));
		}
		trans.setScale(new javax.vecmath.Vector3d(c.getBaseRadius(), c.getAxisLength(), c.getBaseRadius()));
		trans.setTranslation(toJ3DVec(c.getCenter().toVector()));

		((TransformGroup)shapeTransforms.get(c).getChild(0)).setTransform(trans);
	}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
/** get the rotation out of Jmol 
 * 
 * @return the jmol rotation matrix
 */
public Matrix getJmolRotation(){
    	Matrix jmolRotation = Matrix.identity(3, 3);
   
        //structurePanel.executeCmd("show orientation;");
    	JmolViewer jmol = getViewer();
    	Object obj = jmol.getProperty(null,"transformInfo","");
    	// System.out.println(obj);
    	if ( obj instanceof Matrix3f ) {
    		Matrix3f max = (Matrix3f) obj;
    		jmolRotation = new Matrix(3,3);
    		for (int x=0; x<3;x++) {
    			for (int y=0 ; y<3;y++){
    				float val = max.getElement(x,y);
    				// System.out.println("x " + x + " y " + y + " " + val);
    				jmolRotation.set(x,y,val);
    			}
    		}                
    	}                               
    return jmolRotation;
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public static void matrixToFloatBuffer(Matrix3f m, FloatBuffer fb) {
    Matrix3f tempMatrix = new Matrix3f();
    tempMatrix.transpose(m);

    fb.put(tempMatrix.m00);
    fb.put(tempMatrix.m01);
    fb.put(tempMatrix.m02);
    fb.put(tempMatrix.m10);
    fb.put(tempMatrix.m11);
    fb.put(tempMatrix.m12);
    fb.put(tempMatrix.m20);
    fb.put(tempMatrix.m21);
    fb.put(tempMatrix.m22);

    fb.flip();
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
public BulletPhysics(WorldProvider world) {
    collisionGroupManager = CoreRegistry.get(CollisionGroupManager.class);

    _broadphase = new DbvtBroadphase();
    _broadphase.getOverlappingPairCache().setInternalGhostPairCallback(new GhostPairCallback());
    _defaultCollisionConfiguration = new DefaultCollisionConfiguration();
    _dispatcher = new CollisionDispatcher(_defaultCollisionConfiguration);
    _sequentialImpulseConstraintSolver = new SequentialImpulseConstraintSolver();
    _discreteDynamicsWorld = new DiscreteDynamicsWorld(_dispatcher, _broadphase, _sequentialImpulseConstraintSolver, _defaultCollisionConfiguration);
    _discreteDynamicsWorld.setGravity(new Vector3f(0f, -15f, 0f));
    blockEntityRegistry = CoreRegistry.get(BlockEntityRegistry.class);
    CoreRegistry.get(EventSystem.class).registerEventReceiver(this, BlockChangedEvent.class, BlockComponent.class);

    PhysicsWorldWrapper wrapper = new PhysicsWorldWrapper(world);
    VoxelWorldShape worldShape = new VoxelWorldShape(wrapper);

    Matrix3f rot = new Matrix3f();
    rot.setIdentity();
    DefaultMotionState blockMotionState = new DefaultMotionState(new Transform(new Matrix4f(rot, new Vector3f(0, 0, 0), 1.0f)));
    RigidBodyConstructionInfo blockConsInf = new RigidBodyConstructionInfo(0, blockMotionState, worldShape, new Vector3f());
    RigidBody rigidBody = new RigidBody(blockConsInf);
    rigidBody.setCollisionFlags(CollisionFlags.STATIC_OBJECT | rigidBody.getCollisionFlags());
    _discreteDynamicsWorld.addRigidBody(rigidBody, combineGroups(StandardCollisionGroup.WORLD), (short)(CollisionFilterGroups.ALL_FILTER ^ CollisionFilterGroups.STATIC_FILTER));
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
private static Matrix3f roundAndReduceMatrixElements(Matrix4f m) {
	Preconditions.checkArgument(m.m30 == 0);
	Preconditions.checkArgument(m.m31 == 0);
	Preconditions.checkArgument(m.m32 == 0);

	Preconditions.checkArgument(m.m03 == 0);
	Preconditions.checkArgument(m.m13 == 0);
	Preconditions.checkArgument(m.m23 == 0);

	Preconditions.checkArgument(m.m33 == 1);

	final Matrix3f result = new Matrix3f();
	result.m00 = Math.round(m.m00);
	result.m01 = Math.round(m.m01);
	result.m02 = Math.round(m.m02);

	result.m10 = Math.round(m.m10);
	result.m11 = Math.round(m.m11);
	result.m12 = Math.round(m.m12);

	result.m20 = Math.round(m.m20);
	result.m21 = Math.round(m.m21);
	result.m22 = Math.round(m.m22);
	return result;
}
 

import javax.vecmath.Matrix3f; //导入依赖的package包/类
/**
 * BodyのWorld座標系での回転行列(pitch, rollのみ)を返します.
 *
 * @param context
 * @param mat
 */
private void calculateBodyRotation(SomaticContext context, Matrix3f mat) {
	// FIXME 未テスト
	Matrix3f rot;
	if (context.isLeftOnGround())
		rot = context.get(Frames.LSole).getBodyRotation();
	else if (context.isRightOnGround())
		rot = context.get(Frames.RSole).getBodyRotation();
	else {
		mat.setIdentity();
		return;
	}
	Vector3f tmp = new Vector3f();
	MatrixUtils.rot2pyr(rot, tmp);
	tmp.y = 0;
	MatrixUtils.pyr2rot(tmp, mat);
}