Java Matrix3f.transpose方法代码示例

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包/类
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 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 updateInertiaTensor() {
	Matrix3f mat1 = Stack.alloc(Matrix3f.class);
	MatrixUtil.scale(mat1, worldTransform.basis, invInertiaLocal);

	Matrix3f mat2 = Stack.alloc(worldTransform.basis);
	mat2.transpose();

	invInertiaTensorWorld.mul(mat1, mat2);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
protected void buildAngularJacobian(/*JacobianEntry jacAngular*/int jacAngular_index, Vector3f jointAxisW) {
	Matrix3f mat1 = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class)).basis;
	mat1.transpose();

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

	jacAng[jacAngular_index].init(jointAxisW,
			mat1,
			mat2,
			rbA.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
			rbB.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)));
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public static Matrix3f calcNormalMatrix(Matrix4f mv) {
    Matrix3f result = new Matrix3f();
    result.m00 = mv.m00;
    result.m10 = mv.m10;
    result.m20 = mv.m20;
    result.m01 = mv.m01;
    result.m11 = mv.m11;
    result.m21 = mv.m21;
    result.m02 = mv.m02;
    result.m12 = mv.m12;
    result.m22 = mv.m22;

    result.invert(); result.transpose();
    return result;
}
 

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包/类
private static Quat4f stepJacobi(Matrix3f m)
{
    Matrix3f t = new Matrix3f();
    Quat4f qt = new Quat4f(), ret = new Quat4f(0, 0, 0, 1);
    Pair<Float, Float> p;
    // 01
    if(m.m01 * m.m01 + m.m10 * m.m10 > eps)
    {
        p = approxGivensQuat(m.m00, .5f * (m.m01 + m.m10), m.m11);
        qt.set(0, 0, p.getLeft(), p.getRight());
        //qt.normalize();
        ret.mul(qt);
        //t.set(qt);
        t.setIdentity();
        t.m00 = qt.w * qt.w - qt.z * qt.z;
        t.m11 = t.m00;
        t.m10 = 2 * qt.z * qt.w;
        t.m01 = -t.m10;
        t.m22 = qt.w * qt.w + qt.z * qt.z;
        m.mul(m, t);
        t.transpose();
        m.mul(t, m);
    }
    // 02
    if(m.m02 * m.m02 + m.m20 * m.m20 > eps)
    {
        p = approxGivensQuat(m.m00, .5f * (m.m02 + m.m20), m.m22);
        qt.set(0, -p.getLeft(), 0, p.getRight());
        //qt.normalize();
        ret.mul(qt);
        //t.set(qt);
        t.setIdentity();
        t.m00 = qt.w * qt.w - qt.y * qt.y;
        t.m22 = t.m00;
        t.m20 = -2 * qt.y * qt.w;
        t.m02 = -t.m20;
        t.m11 = qt.w * qt.w + qt.y * qt.y;
        m.mul(m, t);
        t.transpose();
        m.mul(t, m);
    }
    // 12
    if(m.m12 * m.m12 + m.m21 * m.m21 > eps)
    {
        p = approxGivensQuat(m.m11, .5f * (m.m12 + m.m21), m.m22);
        qt.set(p.getLeft(), 0, 0, p.getRight());
        //qt.normalize();
        ret.mul(qt);
        //t.set(qt);
        t.setIdentity();
        t.m11 = qt.w * qt.w - qt.x * qt.x;
        t.m22 = t.m11;
        t.m21 = 2 * qt.x * qt.w;
        t.m12 = -t.m21;
        t.m00 = qt.w * qt.w + qt.x * qt.x;
        m.mul(m, t);
        t.transpose();
        m.mul(t, m);
    }
    return ret;
}
 

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包/类
@Override
public void buildJacobian() {
	appliedImpulse = 0f;

	Vector3f normal = Stack.alloc(Vector3f.class);
	normal.set(0f, 0f, 0f);

	Matrix3f tmpMat1 = Stack.alloc(Matrix3f.class);
	Matrix3f tmpMat2 = Stack.alloc(Matrix3f.class);
	Vector3f tmp1 = Stack.alloc(Vector3f.class);
	Vector3f tmp2 = Stack.alloc(Vector3f.class);
	Vector3f tmpVec = Stack.alloc(Vector3f.class);
	
	Transform centerOfMassA = rbA.getCenterOfMassTransform(Stack.alloc(Transform.class));
	Transform centerOfMassB = rbB.getCenterOfMassTransform(Stack.alloc(Transform.class));

	for (int i = 0; i < 3; i++) {
		VectorUtil.setCoord(normal, i, 1f);

		tmpMat1.transpose(centerOfMassA.basis);
		tmpMat2.transpose(centerOfMassB.basis);

		tmp1.set(pivotInA);
		centerOfMassA.transform(tmp1);
		tmp1.sub(rbA.getCenterOfMassPosition(tmpVec));

		tmp2.set(pivotInB);
		centerOfMassB.transform(tmp2);
		tmp2.sub(rbB.getCenterOfMassPosition(tmpVec));

		jac[i].init(
				tmpMat1,
				tmpMat2,
				tmp1,
				tmp2,
				normal,
				rbA.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
				rbA.getInvMass(),
				rbB.getInvInertiaDiagLocal(Stack.alloc(Vector3f.class)),
				rbB.getInvMass());
		VectorUtil.setCoord(normal, i, 0f);
	}
}
 

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包/类
/**
 * Body座標系のベクトルsrcを回転し、to座標系からみたベクトルの向きを返します.
 *
 * @param context
 *            計算に使用するSomaticContext
 * @param to
 *            変換先の座標系
 * @param src
 *            Body座標系内のベクトル
 * @param dest
 *            to座標系に変換した結果のベクトル
 */
public static void toFrameRotation(SomaticContext context, Frames to,
		Vector3f src, Vector3f dest) {
	FrameState fs = context.get(to);
	Matrix3f mat = new Matrix3f();
	mat.transpose(fs.getBodyRotation());
	mat.transform(src, dest);
}