Java Matrix3f.transform方法代码示例

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包/类
/**
 * 重心位置の計算. 再帰呼び出し用.
 *
 * @param ss
 */
private static void calcCenterOfMassRecursively(SomaticContext ss,
		Frames id, Vector3f com) {
	log.trace("calculate CoM recursively");
	RobotFrame rf = ss.getRobot().get(id);
	FrameState fs = ss.get(id);

	Vector3f frameCom = fs.getBodyCenterOfMass();
	Matrix3f mat = fs.getBodyRotation();
	mat.transform(rf.getCenterOfMass(), frameCom);
	frameCom.add(fs.getBodyPosition());
	com.scaleAdd(rf.getMass(), frameCom, com);

	// 子フレームがあれば再帰的に計算する
	if (rf.getChildren() != null)
		for (RobotFrame child : rf.getChildren())
			calcCenterOfMassRecursively(ss, child.getId(), com);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public void testRpy2rot2() throws Exception {
	Vector3f rpy = new Vector3f(MathUtils.toRadians(0), MathUtils
			.toRadians(30), MathUtils.toRadians(0));
	Matrix3f rot = new Matrix3f();
	MatrixUtils.pyr2rot(rpy, rot);

	Vector3f v = new Vector3f(5, 10, 0);
	rot.transform(v);
	System.out.println(v);
	assertEquals(new Vector3f(5, 8.660254f, 5.0f), v, MathUtils.EPSf);

	rpy = new Vector3f(MathUtils.toRadians(30), MathUtils.toRadians(0),
			MathUtils.toRadians(0));
	MatrixUtils.pyr2rot(rpy, rot);

	v = new Vector3f(0, 10, 0);
	rot.transform(v);
	System.out.println(v);
	assertEquals(new Vector3f(-5, 8.660254f, 0), v, MathUtils.EPSf);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
void setRotationMatrix(Matrix3f rotationMatrix) {
    for (int i = normixCount; --i >= 0; ) {
      Vector3f tv = transformedVectors[i];
      rotationMatrix.transform(vertexVectors[i], tv);
//        if (i == 0)
  //        System.out.println(i + " " + shadeIndexes[i]);

      shadeIndexes[i] = Shade3D.getShadeIndexNormalized(tv.x, -tv.y, tv.z);
    //  if (i == 0 || i == 219 || i == 162 || i == 193)
      //  System.out.println(i + " " + shadeIndexes[i]);
      shadeIndexes2Sided[i] = (tv.z >= 0 ? shadeIndexes[i] 
          : Shade3D.getShadeIndexNormalized(-tv.x, tv.y, -tv.z));
    }
  }
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
protected static MeshSurface getConeMesh(Point3f centerBase, Matrix3f matRotateScale, short colix) {
  MeshSurface ms = new MeshSurface();
  int ndeg = 10;
  int n = 360 / ndeg;
  ms.colix = colix;
  ms.vertices = new Point3f[ms.vertexCount = n + 1];
  ms.polygonIndexes = new int[ms.polygonCount = n][];
  for (int i = 0; i < n; i++)
    ms.polygonIndexes[i] = new int[] {i, (i + 1) % n, n };
  double d = ndeg / 180. * Math.PI; 
  for (int i = 0; i < n; i++) {
    float x = (float) (Math.cos(i * d));
    float y = (float) (Math.sin(i * d));
    ms.vertices[i] = new Point3f(x, y, 0);
  }
  ms.vertices[n] = new Point3f(0, 0, 1);
  if (matRotateScale != null) {
    ms.normals = new Vector3f[ms.vertexCount];
    for (int i = 0; i < ms.vertexCount; i++) {
      matRotateScale.transform(ms.vertices[i]);
      ms.normals[i] = new Vector3f();
      ms.normals[i].set(ms.vertices[i]);
      ((Vector3f) ms.normals[i]).normalize();
      ms.vertices[i].add(centerBase);
    }
  }
  return ms;
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public void invXform(Vector3f inVec, Vector3f out) {
	out.sub(inVec, origin);

	Matrix3f mat = Stack.alloc(basis);
	mat.transpose();
	mat.transform(out);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public void transformTriangle(vTriangle t){
	//build matrix to feed these few points.
	//t.v1 = ;
	//new v1 = ptransform + l1
	//Quat4f quatRot = new Quat4f();
	//t.pTransform.getRotation(quatRot);
	Matrix3f tmpMat = new Matrix3f();
	tmpMat.set(t.pDifference.basis);

	Vector3f v1 = add(t.pDifference.origin, t.translate);
	Vector3f v2 = add(t.pDifference.origin, t.translate);
	Vector3f v3 = add(t.pDifference.origin, t.translate);

	Vector3f r1 = (Vector3f) t.l1.clone();
	Vector3f r2 = (Vector3f) t.l2.clone();
	Vector3f r3 = (Vector3f) t.l3.clone();

	tmpMat.transform(r1);
	tmpMat.transform(r2);
	tmpMat.transform(r3);

	t.v1 = add(v1,r1);
	t.v2 = add(v2,r2);
	t.v3 = add(v3,r3);

	//t.v1 = add(v1,r1);
	//t.v2 = add(v2,r2);
	//t.v3 = add(v3,r3);

	//t.pTransform.transform(t.v1);
	//t.pTransform.transform(t.v2);
	//t.pTransform.transform(t.v3);
	//transforming
	//t.v1 = add(t.pTransform.origin, t.v1);
	//t.v1 = add(t.translate, t.v1);
	//t.v2 = add(t.pTransform.origin, t.v2);
	//t.v2 = add(t.translate, t.v2);
	//t.v3 = add(t.pTransform.origin, t.v3);
	//t.v3 = add(t.translate, t.v3);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
private void dropItem(float speedMultiplier) {
	if (!(world instanceof WorldServer)) return;

	for (int i = 0; i < inventory.getSizeInventory(); i++) {
		ItemStack stack = inventory.getStackInSlot(i);
		if (stack.isEmpty()) continue;

		final ItemStack dropped = stack.splitStack(1);

		final Orientation orientation = getOrientation();

		final Vector4f worldDrop = new Vector4f();
		{
			final Matrix4f blockLocalToWorld = orientation.getBlockLocalToWorldMatrix();
			blockLocalToWorld.transform(DROP_POS, worldDrop);
		}

		final Vector3f worldVel = new Vector3f();
		{
			final Matrix3f localToWorld = orientation.getLocalToWorldMatrix();
			worldVel.set(DROP_V);
			worldVel.scale(speedMultiplier);
			localToWorld.transform(worldVel);
		}

		final double worldDropX = pos.getX() + worldDrop.x;
		final double worldDropY = pos.getY() + worldDrop.y;
		final double worldDropZ = pos.getZ() + worldDrop.z;

		final DropItemAction action = new DropItemAction(dropped, worldDropX, worldDropY, worldDropZ, worldVel.x, worldVel.y, worldVel.z);
		FakePlayerPool.instance.executeOnPlayer((WorldServer)world, action);

		break;
	}
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public static void inverseTransform(Vector3f vector, FrameState frame) {
	vector.sub(frame.getPosition());
	Matrix3f mat = new Matrix3f();
	mat.set(frame.getAxisAngle());
	mat.transpose();
	mat.transform(vector);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public void testCalculateBodyRotation() {
	SomaticContext context = new SomaticContext(robot);
	Matrix3f mat = new Matrix3f();
	mat.set(context.get(Frames.LSole).getBodyRotation());
	mat.set(context.get(Frames.RSole).getBodyRotation());
	Vector3f v = new Vector3f(0, 0, 1);
	mat.transform(v);
	System.out.println(v);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public static void getEquationForQuadricWithCenter(float x, float y, float z, Matrix3f mToElliptical, 
                                           Vector3f vTemp, Matrix3f mTemp, double[] coef, Matrix4f mDeriv) {
  /* Starting with a center point and a matrix that converts cartesian 
   * or screen coordinates to ellipsoidal coordinates, 
   * this method fills a float[10] with the terms for the 
   * equation for the ellipsoid:
   * 
   * c0 x^2 + c1 y^2 + c2 z^2 + c3 xy + c4 xz + c5 yz + c6 x + c7 y + c8 z - 1 = 0 
   * 
   * I made this up; I haven't seen it in print. -- Bob Hanson, 4/2008
   * 
   */
  
  vTemp.set(x, y, z);
  mToElliptical.transform(vTemp);
  double f = 1 - vTemp.dot(vTemp); // J
  mTemp.transpose(mToElliptical);
  mTemp.transform(vTemp);
  mTemp.mul(mToElliptical);
  coef[0] = mTemp.m00 / f;     // A = aXX
  coef[1] = mTemp.m11 / f;     // B = aYY
  coef[2] = mTemp.m22 / f;     // C = aZZ
  coef[3] = mTemp.m01 * 2 / f; // D = aXY
  coef[4] = mTemp.m02 * 2 / f; // E = aXZ
  coef[5] = mTemp.m12 * 2 / f; // F = aYZ
  coef[6] = -2 * vTemp.x / f;  // G = aX
  coef[7] = -2 * vTemp.y / f;  // H = aY
  coef[8] = -2 * vTemp.z / f;  // I = aZ
  coef[9] = -1;                // J = -1
  
  /*
   * f = Ax^2 + By^2 + Cz^2 + Dxy + Exz + Fyz + Gx + Hy + Iz + J
   * df/dx = 2Ax +  Dy +  Ez + G
   * df/dy =  Dx + 2By +  Fz + H
   * df/dz =  Ex +  Fy + 2Cz + I
   */
  
  if (mDeriv == null)
    return;
  mDeriv.setIdentity();
  mDeriv.m00 = (float) (2 * coef[0]);
  mDeriv.m11 = (float) (2 * coef[1]);
  mDeriv.m22 = (float) (2 * coef[2]);

  mDeriv.m01 = mDeriv.m10 = (float) coef[3];
  mDeriv.m02 = mDeriv.m20 = (float) coef[4];
  mDeriv.m12 = mDeriv.m21 = (float) coef[5];

  mDeriv.m03 = (float) coef[6];
  mDeriv.m13 = (float) coef[7];
  mDeriv.m23 = (float) coef[8];
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
/**
 * Bilateral constraint between two dynamic objects.
 */
public static void resolveSingleBilateral(RigidBody body1, Vector3f pos1,
		RigidBody body2, Vector3f pos2,
		float distance, Vector3f normal, float[] impulse, float timeStep) {
	float normalLenSqr = normal.lengthSquared();
	assert (Math.abs(normalLenSqr) < 1.1f);
	if (normalLenSqr > 1.1f) {
		impulse[0] = 0f;
		return;
	}

	ObjectPool<JacobianEntry> jacobiansPool = ObjectPool.get(JacobianEntry.class);
	Vector3f tmp = Stack.alloc(Vector3f.class);
	
	Vector3f rel_pos1 = Stack.alloc(Vector3f.class);
	rel_pos1.sub(pos1, body1.getCenterOfMassPosition(tmp));

	Vector3f rel_pos2 = Stack.alloc(Vector3f.class);
	rel_pos2.sub(pos2, body2.getCenterOfMassPosition(tmp));

	//this jacobian entry could be re-used for all iterations

	Vector3f vel1 = Stack.alloc(Vector3f.class);
	body1.getVelocityInLocalPoint(rel_pos1, vel1);

	Vector3f vel2 = Stack.alloc(Vector3f.class);
	body2.getVelocityInLocalPoint(rel_pos2, vel2);

	Vector3f vel = Stack.alloc(Vector3f.class);
	vel.sub(vel1, vel2);

	Matrix3f mat1 = body1.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
	mat1.transpose();

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

	JacobianEntry jac = jacobiansPool.get();
	jac.init(mat1, mat2,
			rel_pos1, rel_pos2, normal,
			body1.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)), body1.getInvMass(),
			body2.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)), body2.getInvMass());

	float jacDiagAB = jac.getDiagonal();
	float jacDiagABInv = 1f / jacDiagAB;

	Vector3f tmp1 = body1.getAngularVelocity(Stack.alloc(Vector3f.class));
	mat1.transform(tmp1);

	Vector3f tmp2 = body2.getAngularVelocity(Stack.alloc(Vector3f.class));
	mat2.transform(tmp2);

	float rel_vel = jac.getRelativeVelocity(
			body1.getLinearVelocity(Stack.alloc(Vector3f.class)),
			tmp1,
			body2.getLinearVelocity(Stack.alloc(Vector3f.class)),
			tmp2);

	jacobiansPool.release(jac);

	float a;
	a = jacDiagABInv;


	rel_vel = normal.dot(vel);

	// todo: move this into proper structure
	float contactDamping = 0.2f;

	//#ifdef ONLY_USE_LINEAR_MASS
	//	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
	//	impulse = - contactDamping * rel_vel * massTerm;
	//#else	
	float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
	impulse[0] = velocityImpulse;
	//#endif
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public static Vector3f times(Matrix3f transform, Vector3f vec) {
    Vector3f dest = new Vector3f(vec);
    transform.transform(dest);
    return dest;
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public static void body2robotCoord(SomaticContext context, Vector3f src,
		Vector3f dest) {
	Matrix3f rot = new Matrix3f();
	rot.transpose(context.getBodyPosture());
	rot.transform(src, dest);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public static void robot2bodyCoord(SomaticContext context, Vector3f src,
		Vector3f dest) {
	Matrix3f rot = context.getBodyPosture();
	rot.transform(src, dest);
}