Java Matrix3.transpose方法代码示例

import com.badlogic.gdx.math.Matrix3; //导入方法依赖的package包/类
protected void buildLinearJacobian (/* JacobianEntry jacLinear */int jacLinear_index, Vector3 normalWorld, Vector3 pivotAInW,
	Vector3 pivotBInW) {
	Stack stack = Stack.enter();
	Matrix3 mat1 = rbA.getCenterOfMassTransform(stack.allocTransform()).basis;
	mat1.transpose();

	Matrix3 mat2 = rbB.getCenterOfMassTransform(stack.allocTransform()).basis;
	mat2.transpose();

	Vector3 tmpVec = stack.allocVector3();

	Vector3 tmp1 = stack.allocVector3();
	tmp1.set(pivotAInW).sub(rbA.getCenterOfMassPosition(tmpVec));

	Vector3 tmp2 = stack.allocVector3();
	tmp2.set(pivotBInW).sub(rbB.getCenterOfMassPosition(tmpVec));

	jacLinear[jacLinear_index].init(mat1, mat2, tmp1, tmp2, normalWorld, rbA.getInvInertiaDiagLocal(stack.allocVector3()),
		rbA.getInvMass(), rbB.getInvInertiaDiagLocal(stack.allocVector3()), rbB.getInvMass());
	stack.leave();
}
 

import com.badlogic.gdx.math.Matrix3; //导入方法依赖的package包/类
public void invXform(Vector3 inVec, Vector3 out) {
	out.set(inVec).sub(origin);
	Stack stack = Stack.enter();
	Matrix3 mat = stack.alloc(basis);
	mat.transpose();
	out.mul(mat);
	stack.leave();
}
 

import com.badlogic.gdx.math.Matrix3; //导入方法依赖的package包/类
public void updateInertiaTensor () {
	Stack stack = Stack.enter();
	Matrix3 mat1 = stack.allocMatrix3();
	MatrixUtil.scale(mat1, worldTransform.basis, invInertiaLocal);

	Matrix3 mat2 = stack.alloc(worldTransform.basis);
	mat2.transpose();

	invInertiaTensorWorld.set(mat1).mul(mat2);
	stack.leave();
}
 

import com.badlogic.gdx.math.Matrix3; //导入方法依赖的package包/类
protected void buildAngularJacobian (/* JacobianEntry jacAngular */int jacAngular_index, Vector3 jointAxisW) {
	Stack stack = Stack.enter();
	Matrix3 mat1 = rbA.getCenterOfMassTransform(stack.allocTransform()).basis;
	mat1.transpose();

	Matrix3 mat2 = rbB.getCenterOfMassTransform(stack.allocTransform()).basis;
	mat2.transpose();

	jacAng[jacAngular_index].init(jointAxisW, mat1, mat2, rbA.getInvInertiaDiagLocal(stack.allocVector3()),
		rbB.getInvInertiaDiagLocal(stack.allocVector3()));
	stack.leave();
}
 

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

	ObjectPool<JacobianEntry> jacobiansPool = ObjectPool.get(JacobianEntry.class, new Supplier<JacobianEntry>() {
		@Override
		public JacobianEntry get () {
			return new JacobianEntry();
		}
	});
	Stack stack = Stack.enter();
	Vector3 tmp = stack.allocVector3();

	Vector3 rel_pos1 = stack.allocVector3();
	rel_pos1.set(pos1).sub(body1.getCenterOfMassPosition(tmp));

	Vector3 rel_pos2 = stack.allocVector3();
	rel_pos2.set(pos2).sub(body2.getCenterOfMassPosition(tmp));

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

	Vector3 vel1 = stack.allocVector3();
	body1.getVelocityInLocalPoint(rel_pos1, vel1);

	Vector3 vel2 = stack.allocVector3();
	body2.getVelocityInLocalPoint(rel_pos2, vel2);

	Vector3 vel = stack.allocVector3();
	vel.set(vel1).sub(vel2);

	Matrix3 mat1 = body1.getCenterOfMassTransform(stack.allocTransform()).basis;
	mat1.transpose();

	Matrix3 mat2 = body2.getCenterOfMassTransform(stack.allocTransform()).basis;
	mat2.transpose();

	JacobianEntry jac = jacobiansPool.get();
	jac.init(mat1, mat2, rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(stack.allocVector3()), body1.getInvMass(),
		body2.getInvInertiaDiagLocal(stack.allocVector3()), body2.getInvMass());

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

	Vector3 tmp1 = body1.getAngularVelocity(stack.allocVector3());
	tmp1.mul(mat1);

	Vector3 tmp2 = body2.getAngularVelocity(stack.allocVector3());
	tmp2.mul(mat2);

	float rel_vel = jac.getRelativeVelocity(body1.getLinearVelocity(stack.allocVector3()), tmp1,
		body2.getLinearVelocity(stack.allocVector3()), 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
	stack.leave();
}