Java Matrix3f.set方法代码示例

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 testRot2omega() throws Exception {
	Vector3f pyr = new Vector3f(MathUtils.toRadians(0), MathUtils
			.toRadians(20), MathUtils.toRadians(20));
	Matrix3f rot = new Matrix3f();
	MatrixUtils.pyr2rot(pyr, rot);
	System.out.println(rot);

	Vector3f omega = new Vector3f();
	MatrixUtils.rot2omega(rot, omega);
	Matrix3f a = new Matrix3f();
	a.set(new AxisAngle4f(omega, omega.length()));
	System.out.println(pyr);
	System.out.println(omega);
	System.out.println(a);

	MatrixUtils.rot2pyr(rot, pyr);
	System.out.println(pyr);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public void set(AxisAngle4f a) {
  AxisAngle4f aa = new AxisAngle4f(a);
  if (aa.angle == 0)
    aa.y = 1;
  Matrix3f m3 = new Matrix3f();
  m3.set(aa);
  set(m3);
}
 

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

	Vector3f halfExtents = Stack.alloc(Vector3f.class);
	halfExtents.set(getRadius(), getRadius(), getRadius());
	VectorUtil.setCoord(halfExtents, upAxis, getRadius() + getHalfHeight());

	halfExtents.x += getMargin();
	halfExtents.y += getMargin();
	halfExtents.z += getMargin();

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

	Vector3f center = 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);

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

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public static void calculateDiffAxisAngle(Transform transform0, Transform transform1, Vector3f axis, float[] angle) {
// #ifdef USE_QUATERNION_DIFF
//		btQuaternion orn0 = transform0.getRotation();
//		btQuaternion orn1a = transform1.getRotation();
//		btQuaternion orn1 = orn0.farthest(orn1a);
//		btQuaternion dorn = orn1 * orn0.inverse();
// #else
		Matrix3f tmp = Stack.alloc(Matrix3f.class);
		tmp.set(transform0.basis);
		MatrixUtil.invert(tmp);

		Matrix3f dmat = Stack.alloc(Matrix3f.class);
		dmat.mul(transform1.basis, tmp);

		Quat4f dorn = Stack.alloc(Quat4f.class);
		MatrixUtil.getRotation(dmat, dorn);
// #endif

		// floating point inaccuracy can lead to w component > 1..., which breaks 

		dorn.normalize();

		angle[0] = QuaternionUtil.getAngle(dorn);
		axis.set(dorn.x, dorn.y, dorn.z);
		// TODO: probably not needed
		//axis[3] = btScalar(0.);

		// check for axis length
		float len = axis.lengthSquared();
		if (len < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
			axis.set(1f, 0f, 0f);
		}
		else {
			axis.scale(1f / (float) Math.sqrt(len));
		}
	}
 

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

	Vector3f halfExtents = Stack.alloc(Vector3f.class);
	halfExtents.set(getRadius(), getRadius(), getRadius());
	VectorUtil.setCoord(halfExtents, upAxis, getRadius() + getHalfHeight());

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

	Vector3f center = 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);

	extent.x += getMargin();
	extent.y += getMargin();
	extent.z += getMargin();

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

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
/**
 * Places the rotational component of the camera transform in <code>result</code> and returns it.
 */
public Matrix3f getRotation( Matrix3f result )
{
	updateRotation( );
	result.set( rotation );
	return result;
}
 

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 static Multimap<Orientation, XYZRotation> calculateXyzRotations(Map<Matrix3f, Orientation> fromMatrix) {
	final Multimap<Orientation, XYZRotation> toXYZRotation = HashMultimap.create();
	for (Rotation x : Rotation.values())
		for (Rotation y : Rotation.values())
			for (Rotation z : Rotation.values()) {
				final XYZRotation rotation = new XYZRotation(x, y, z);
				Quat4f q = new Quat4f(0, 0, 0, 1);

				Quat4f tmp = new Quat4f();
				tmp.set(new AxisAngle4f(0, 0, 1, z.angle));
				q.mul(tmp);

				tmp.set(new AxisAngle4f(0, 1, 0, y.angle));
				q.mul(tmp);

				tmp.set(new AxisAngle4f(1, 0, 0, x.angle));
				q.mul(tmp);

				Matrix3f m = new Matrix3f();
				m.set(q);
				roundMatrixElements(m);
				final Orientation orientation = fromMatrix.get(m);
				Preconditions.checkNotNull(orientation, rotation);
				toXYZRotation.put(orientation, rotation);
			}

	return toXYZRotation;
}
 

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包/类
void getRotation(Matrix3f matrixRotation) {
  // hmm ... I suppose that there could be a race condiditon here
  // if matrixRotate is being modified while this is called
  matrixRotation.set(matrixRotate);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public Matrix3f getInvInertiaTensorWorld(Matrix3f out) {
	out.set(invInertiaTensorWorld);
	return out;
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
/**
 * Calcs the euler angles between the two bodies.
 */
protected void calculateAngleInfo() {
	Matrix3f mat = Stack.alloc(Matrix3f.class);

	Matrix3f relative_frame = Stack.alloc(Matrix3f.class);
	mat.set(calculatedTransformA.basis);
	MatrixUtil.invert(mat);
	relative_frame.mul(mat, calculatedTransformB.basis);

	matrixToEulerXYZ(relative_frame, calculatedAxisAngleDiff);

	// in euler angle mode we do not actually constrain the angular velocity
	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
	//
	//    to get			constrain w2-w1 along		...not
	//    ------			---------------------		------
	//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
	//    d(angle[1])/dt = 0	ax[1]
	//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
	//
	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
	// to prove the result for angle[0], write the expression for angle[0] from
	// GetInfo1 then take the derivative. to prove this for angle[2] it is
	// easier to take the euler rate expression for d(angle[2])/dt with respect
	// to the components of w and set that to 0.

	Vector3f axis0 = Stack.alloc(Vector3f.class);
	calculatedTransformB.basis.getColumn(0, axis0);

	Vector3f axis2 = Stack.alloc(Vector3f.class);
	calculatedTransformA.basis.getColumn(2, axis2);

	calculatedAxis[1].cross(axis2, axis0);
	calculatedAxis[0].cross(calculatedAxis[1], axis2);
	calculatedAxis[2].cross(axis0, calculatedAxis[1]);

	//    if(m_debugDrawer)
	//    {
	//
	//    	char buff[300];
	//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
	//		m_calculatedAxisAngleDiff[0],
	//		m_calculatedAxisAngleDiff[1],
	//		m_calculatedAxisAngleDiff[2]);
	//    	m_debugDrawer->reportErrorWarning(buff);
	//    }
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public static Matrix3f rotationMatrix(AxisAngle4f axisAngle) {
	Matrix3f mat = new Matrix3f();
	mat.set(axisAngle);
	return mat;
}