Java Matrix3f.mul方法代码示例

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包/类
private static BakedModelKnowledgeBook rebake(ModelKnowledgeBook model, String name) {	
	Matrix4f m = new Matrix4f();
       m.m20 = 1f / 128f;
       m.m01 = m.m12 = -m.m20;
       m.m33 = 1;
       Matrix3f rotation = new Matrix3f();
       m.getRotationScale(rotation);
       Matrix3f angleZ = new Matrix3f();
       angleZ.rotZ(-1.5708F);
       rotation.mul(rotation, angleZ);
       m.setRotationScale(rotation);
       m.setScale(0.66666666667F * m.getScale());
       m.setTranslation(new Vector3f(0.1875F, 0.2505F, 0.125F));
	SimpleModelFontRenderer fontRenderer = new SimpleModelFontRenderer(Minecraft.getMinecraft().gameSettings, font, Minecraft.getMinecraft().getTextureManager(), false, m, DefaultVertexFormats.ITEM) {
		@Override
		protected float renderUnicodeChar(char c, boolean italic) {
			return super.renderDefaultChar(126, italic);
		}
	};
	int maxLineWidth = 96;
	TextureAtlasSprite fontSprite = Minecraft.getMinecraft().getTextureMapBlocks().getAtlasSprite(font2.toString());
	List<BakedQuad> textQuads = new ArrayList<BakedQuad>();
	
	fontRenderer.setSprite(fontSprite);
       fontRenderer.setFillBlanks(false);
       
       int yOffset = 2;
       String title = I18n.translateToLocal(name);
       List<String> lines = fontRenderer.listFormattedStringToWidth(title, maxLineWidth);
       for (int line = 0; line < lines.size(); line++) {
       	int offset = ((maxLineWidth - fontRenderer.getStringWidth(lines.get(line))) / 2);
       	fontRenderer.drawString(lines.get(line), offset, yOffset, 0x00000000);
       	yOffset += (fontRenderer.FONT_HEIGHT - 1 + 4);
       }
	
       textQuads.addAll(fontRenderer.build());
       return new BakedModelKnowledgeBook(model, textQuads);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
protected Matrix3f getRotationMatrix(Point3f pt1, Point3f ptZ, float radius, Point3f ptX, Point3f ptY) {    
  Matrix3f m = new Matrix3f();
  m.m00 = ptX.distance(pt1) * radius;
  m.m11 = ptY.distance(pt1) * radius;
  m.m22 = ptZ.distance(pt1) * 2;
  Quaternion q = Quaternion.getQuaternionFrame(pt1, ptX, ptY);
  Matrix3f m1 = q.getMatrix();
  m1.mul(m);
  return m1;
}
 

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包/类
public void updateWheelTransform(int wheelIndex, boolean interpolatedTransform) {
	WheelInfo wheel = wheelInfo.getQuick(wheelIndex);
	updateWheelTransformsWS(wheel, interpolatedTransform);
	Vector3f up = Stack.alloc(Vector3f.class);
	up.negate(wheel.raycastInfo.wheelDirectionWS);
	Vector3f right = wheel.raycastInfo.wheelAxleWS;
	Vector3f fwd = Stack.alloc(Vector3f.class);
	fwd.cross(up, right);
	fwd.normalize();
	// up = right.cross(fwd);
	// up.normalize();

	// rotate around steering over de wheelAxleWS
	float steering = wheel.steering;

	Quat4f steeringOrn = Stack.alloc(Quat4f.class);
	QuaternionUtil.setRotation(steeringOrn, up, steering); //wheel.m_steering);
	Matrix3f steeringMat = Stack.alloc(Matrix3f.class);
	MatrixUtil.setRotation(steeringMat, steeringOrn);

	Quat4f rotatingOrn = Stack.alloc(Quat4f.class);
	QuaternionUtil.setRotation(rotatingOrn, right, -wheel.rotation);
	Matrix3f rotatingMat = Stack.alloc(Matrix3f.class);
	MatrixUtil.setRotation(rotatingMat, rotatingOrn);

	Matrix3f basis2 = Stack.alloc(Matrix3f.class);
	basis2.setRow(0, right.x, fwd.x, up.x);
	basis2.setRow(1, right.y, fwd.y, up.y);
	basis2.setRow(2, right.z, fwd.z, up.z);

	Matrix3f wheelBasis = wheel.worldTransform.basis;
	wheelBasis.mul(steeringMat, rotatingMat);
	wheelBasis.mul(basis2);

	wheel.worldTransform.origin.scaleAdd(wheel.raycastInfo.suspensionLength, wheel.raycastInfo.wheelDirectionWS, wheel.raycastInfo.hardPointWS);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
public void updateWheelTransform(int wheelIndex, boolean interpolatedTransform) {
	WheelInfo wheel = wheelInfo.get(wheelIndex);
	updateWheelTransformsWS(wheel, interpolatedTransform);
	Vector3f up = Stack.alloc(Vector3f.class);
	up.negate(wheel.raycastInfo.wheelDirectionWS);
	Vector3f right = wheel.raycastInfo.wheelAxleWS;
	Vector3f fwd = Stack.alloc(Vector3f.class);
	fwd.cross(up, right);
	fwd.normalize();
	// up = right.cross(fwd);
	// up.normalize();

	// rotate around steering over de wheelAxleWS
	float steering = wheel.steering;

	Quat4f steeringOrn = Stack.alloc(Quat4f.class);
	QuaternionUtil.setRotation(steeringOrn, up, steering); //wheel.m_steering);
	Matrix3f steeringMat = Stack.alloc(Matrix3f.class);
	MatrixUtil.setRotation(steeringMat, steeringOrn);

	Quat4f rotatingOrn = Stack.alloc(Quat4f.class);
	QuaternionUtil.setRotation(rotatingOrn, right, -wheel.rotation);
	Matrix3f rotatingMat = Stack.alloc(Matrix3f.class);
	MatrixUtil.setRotation(rotatingMat, rotatingOrn);

	Matrix3f basis2 = Stack.alloc(Matrix3f.class);
	basis2.setRow(0, right.x, fwd.x, up.x);
	basis2.setRow(1, right.y, fwd.y, up.y);
	basis2.setRow(2, right.z, fwd.z, up.z);

	Matrix3f wheelBasis = wheel.worldTransform.basis;
	wheelBasis.mul(steeringMat, rotatingMat);
	wheelBasis.mul(basis2);

	wheel.worldTransform.origin.scaleAdd(wheel.raycastInfo.suspensionLength, wheel.raycastInfo.wheelDirectionWS, wheel.raycastInfo.hardPointWS);
}
 

import javax.vecmath.Matrix3f; //导入方法依赖的package包/类
private static BakedModelSpellPage rebake(ModelSpellPage model, Spell spell) {	
	Matrix4f m = new Matrix4f();
       m.m20 = 1f / 128f;
       m.m01 = m.m12 = -m.m20;
       m.m33 = 1;
       Matrix3f rotation = new Matrix3f();
       m.getRotationScale(rotation);
       Matrix3f angle = new Matrix3f();
       angle.rotZ(1.5708F);
       rotation.mul(rotation, angle);
       m.setRotationScale(rotation);
       m.setScale(0.75F * m.getScale());
       m.setTranslation(new Vector3f(0.8625F, 0.095F, 0.8625F));
	SimpleModelFontRenderer fontRenderer = new SimpleModelFontRenderer(Minecraft.getMinecraft().gameSettings, font, Minecraft.getMinecraft().getTextureManager(), false, m, DefaultVertexFormats.ITEM) {
		@Override
		protected float renderUnicodeChar(char c, boolean italic) {
			return super.renderDefaultChar(126, italic);
		}
	};
	TextureAtlasSprite fontSprite = Minecraft.getMinecraft().getTextureMapBlocks().getAtlasSprite(font2.toString());
	List<BakedQuad> textQuads = new ArrayList<BakedQuad>();
	
	fontRenderer.setSprite(fontSprite);
       fontRenderer.setFillBlanks(false);
       String name = TextFormatting.BOLD + I18n.translateToLocal(spell.getUnlocalizedName() + ".name");
       String text = I18n.translateToLocal(spell.getUnlocalizedName() + ".desc");
       String castingType = "--" + I18n.translateToLocal(spell.getCastingType().getName() + ".name") + "--";
       
       int yOffset = 0;
       fontRenderer.drawString(name, (126 - fontRenderer.getStringWidth(name)) / 2, yOffset, 0x007A0000);
       yOffset += (fontRenderer.FONT_HEIGHT + 1);
       List<String> lines = fontRenderer.listFormattedStringToWidth(text, 126);
       for (int line = 0; line < lines.size(); line++) {
       	int offset = ((126 - fontRenderer.getStringWidth(lines.get(line))) / 2);
       	fontRenderer.drawString(lines.get(line), offset, yOffset, 0x004F0000);
       	yOffset += fontRenderer.FONT_HEIGHT;
       }
       yOffset += 5;
       fontRenderer.drawString(castingType, (126 - fontRenderer.getStringWidth(castingType)) / 2, yOffset, 0x00210000);
	
       textQuads.addAll(fontRenderer.build());
       return new BakedModelSpellPage(model, textQuads);
}
 

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包/类
/**
 * 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 void step(SomaticContext sc) {
	boolean isLeftTouched = leftFilter.eval(sc.isLeftOnGround());
	boolean isRightTouched = rightFilter.eval(sc.isRightOnGround());

	// 変位の計算に使う支持脚を決定する
	// FIXME 支持脚の選択が不安定で誤差が大きい.
	boolean useLeft;
	if (isLeftTouched && isRightTouched) {
		// 両方接地 > 圧力の高い方
		float leftp = sc.getLeftPressure();
		float rightp = sc.getRightPressure();
		if (leftp > rightp) {
			useLeft = true;
			log.trace("use left" + leftp + " (" + rightp + ")");
		} else {
			useLeft = false;
			log.trace("use right" + rightp + " (" + leftp + ")");
		}
	} else if (isLeftTouched) {
		useLeft = true;
		log.trace("use left");
	} else if (isRightTouched) {
		useLeft = false;
		log.trace("use right");
	} else {
		// 両足ともついていない
		dx.set(0, 0, 0);
		dr.setIdentity();
		dpyr.set(0, 0, 0);
		return;
	}

	Frames support = useLeft ? Frames.LSole : Frames.RSole;
	Vector3f supportPos = useLeft ? lastLeftPosition : lastRightPosition;
	Matrix3f supportRot = useLeft ? lastLeftRotation : lastRightRotation;
	Frames swing = !useLeft ? Frames.LSole : Frames.RSole;
	Vector3f swingPos = !useLeft ? lastLeftPosition : lastRightPosition;
	Matrix3f swingRot = !useLeft ? lastLeftRotation : lastRightRotation;

	Vector3f pos = sc.get(support).getBodyPosition();
	Matrix3f rot = sc.get(support).getBodyRotation();
	// 支持脚の位置の変化を計算
	dx.set(supportPos);
	Coordinates.body2robotCoord(sc, pos, supportPos);
	dx.sub(supportPos);

	// 支持脚の姿勢の変化を計算
	// pyr2.sub(pyr1)でもよさそう?
	dr.set(supportRot);
	supportRot.mul(sc.getBodyPosture(), rot);
	dr.mulTransposeRight(dr, supportRot);
	// ロールピッチヨー表現に直す
	MatrixUtils.rot2pyr(dr, dpyr);

	// 次の実行のために遊脚も計算(なくてもいい?)
	Coordinates.body2robotCoord(sc, sc.get(swing).getBodyPosition(),
			swingPos);
	swingRot.mul(sc.getBodyPosture(), sc.get(swing).getBodyRotation());
}