Java Code Examples for javax.vecmath.Matrix3d#mul()

/**
 * Convert Euler rotations to a matrix.
 * See also https://www.learnopencv.com/rotation-matrix-to-euler-angles/
 * @param v radian rotation values
 * @return Matrix3d resulting matrix
 */
public static Matrix3d eulerToMatrix(Vector3d v) {
	double c0 = Math.cos(v.x);		double s0 = Math.sin(v.x);
	double c1 = Math.cos(v.y);		double s1 = Math.sin(v.y);
	double c2 = Math.cos(v.z);		double s2 = Math.sin(v.z);
	
	Matrix3d rX=new Matrix3d( 1,  0, 0,
							  0,c0,-s0,
							  0,s0, c0);
	Matrix3d rY=new Matrix3d(c1,  0,s1,
							  0,  1, 0,
							-s1,  0,c1);
	Matrix3d rZ=new Matrix3d(c2,-s2, 0,
			                 s2, c2, 0,
			                  0,  0, 1);

	Matrix3d result = new Matrix3d();
	Matrix3d interim = new Matrix3d();
	interim.mul(rY,rX);
	result.mul(rZ,interim);

	return result;
}
 

/**
 * Rotate all the vertexes by a given amount
 * @param arg0 amount in degrees to rotate around X,Y, and then Z. 
 */
public void adjustRotation(Vector3d arg0) {
	// generate the pose matrix
	Matrix3d pose = new Matrix3d();
	Matrix3d rotX = new Matrix3d();
	Matrix3d rotY = new Matrix3d();
	Matrix3d rotZ = new Matrix3d();
	rotX.rotX((float)Math.toRadians(arg0.x));
	rotY.rotY((float)Math.toRadians(arg0.y));
	rotZ.rotZ((float)Math.toRadians(arg0.z));
	pose.set(rotX);
	pose.mul(rotY);
	pose.mul(rotZ);
	adjust.set(pose);
	isDirty=true;
}
 

public DHTool_GoProCamera() {
	super();
	setName("GoPro Camera");
	flags = LinkAdjust.R;
	
	refreshPoseMatrix();
	
	setModelFilename("/Sixi2/gopro/gopro.stl");
	setModelScale(0.1f);
	setModelOrigin(0, 0, 0.5);
	setModelRotation(90, 90, 0);
	
	// adjust the model's position and rotation.
	this.setPosition(new Vector3d(50,0,50));
	Matrix3d m = new Matrix3d();
	m.setIdentity();
	m.rotX(Math.toRadians(90));
	Matrix3d m2 = new Matrix3d();
	m2.setIdentity();
	m2.rotZ(Math.toRadians(90));
	m.mul(m2);
	this.setRotation(m);
}
 

protected Matrix3d buildPanTiltMatrix(double panDeg,double tiltDeg) {
	Matrix3d a = new Matrix3d();
	Matrix3d b = new Matrix3d();
	Matrix3d c = new Matrix3d();
	a.rotZ(Math.toRadians(panDeg));
	b.rotX(Math.toRadians(-tiltDeg));
	c.mul(b,a);

	right.x=c.m00;
	right.y=c.m01;
	right.z=c.m02;

	up.x=c.m10;
	up.y=c.m11;
	up.z=c.m12;
	
	forward.x=c.m20;
	forward.y=c.m21;
	forward.z=c.m22;
	
	c.transpose();
	
	return c;
}
 

@Override
public Matrix3d getViewMatrix(int index) {
	Matrix3d m = new Matrix3d();
	switch (index) {
	case 0:
		m.setIdentity(); // front vertex-centered
		break;
	case 1:
		m.rotX(-0.6523581397843639); // back face-centered -0.5535743588970415 m.rotX(Math.toRadians(-26));
		break;
	case 2:
		m.rotZ(Math.PI/2);
		Matrix3d m1 = new Matrix3d();
		m1.rotX(-1.0172219678978445);
		m.mul(m1);
		break;
	default:
		throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
	}
	return m;
}
 

@Override
public Matrix3d getViewMatrix(int index) {
	Matrix3d m = new Matrix3d();
	switch (index) {
	case 0:
		m.setIdentity(); // front
		break;
	case 1:
		m.rotX(Math.PI/2); // side edge-centered
		break;
	case 2:
		m.rotY(Math.PI/n); // side face-centered
		Matrix3d m1 = new Matrix3d();
		m1.rotX(Math.PI/2);
		m.mul(m1);
		break;
	case 3:
		m.set(flipX()); // back
		break;
	default:
		throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
	}
	return m;
}
 

@Override
public Matrix3d getViewMatrix(int index) {
	Matrix3d m = new Matrix3d();
	switch (index) {
	case 0:
		m.setIdentity(); // C4 vertex-centered
		break;
	case 1:
		m.rotX(-0.5 * TETRAHEDRAL_ANGLE); // C3 face-centered  2.0*Math.PI/3
		Matrix3d m1 = new Matrix3d();
		m1.rotZ(Math.PI/4);
		m.mul(m1);
		break;
	case 2:
		m.rotY(Math.PI/4); // side face-centered
		break;
	default:
		throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
	}
	return m;
}
 

/**
 * Generates the rotated moment of inertia tensor with the body; uses the following formula: I'
 * = R * I * R-transpose; where I' is the rotated inertia, I is un-rotated interim, and R is the
 * rotation matrix.
 */
private void calculateFramedMOITensor() {
    double[] framedMOI = RotationMatrices.getZeroMatrix(3);

    // Copy the rotation matrix, ignore the translation and scaling parts.
    Matrix3d rotationMatrix = getParent().getShipTransformationManager()
        .getCurrentPhysicsTransform().createRotationMatrix(TransformType.SUBSPACE_TO_GLOBAL);

    Matrix3d inertiaBodyFrame = new Matrix3d(gameMoITensor);
    // The product of the overall rotation matrix with the inertia tensor.
    inertiaBodyFrame.mul(rotationMatrix);
    rotationMatrix.transpose();
    // The product of the inertia tensor multiplied with the transpose of the
    // rotation transpose.
    inertiaBodyFrame.mul(rotationMatrix);
    framedMOI[0] = inertiaBodyFrame.m00;
    framedMOI[1] = inertiaBodyFrame.m01;
    framedMOI[2] = inertiaBodyFrame.m02;
    framedMOI[3] = inertiaBodyFrame.m10;
    framedMOI[4] = inertiaBodyFrame.m11;
    framedMOI[5] = inertiaBodyFrame.m12;
    framedMOI[6] = inertiaBodyFrame.m20;
    framedMOI[7] = inertiaBodyFrame.m21;
    framedMOI[8] = inertiaBodyFrame.m22;

    physMOITensor = framedMOI;
    setPhysInvMOITensor(RotationMatrices.inverse3by3(framedMOI));
}
 

/**
 * Confirms that this matrix is a rotation matrix.  Matrix A * transpose(A) should be the Identity.
 * See also https://www.learnopencv.com/rotation-matrix-to-euler-angles/
 * @param mat
 * @return
 */
public static boolean isRotationMatrix(Matrix3d mat) {
	Matrix3d m1 = new Matrix3d(mat);
	Matrix3d m2 = new Matrix3d();
	m2.transpose(m1);
	m1.mul(m2);
	m2.setIdentity();
	return m1.epsilonEquals(m2, 1e-6);
}
 

/**
 * Use Forward Kinematics to approximate the Jacobian matrix for Sixi.
 * See also https://robotacademy.net.au/masterclass/velocity-kinematics-in-3d/?lesson=346
 */
public double [][] approximateJacobian(DHKeyframe keyframe) {
	double [][] jacobian = new double[6][6];
	
	double ANGLE_STEP_SIZE_DEGREES=0.5;  // degrees
	
	DHKeyframe oldPoseFK = getIKSolver().createDHKeyframe();
	getPoseFK(oldPoseFK);
	
	setPoseFK(keyframe);
	Matrix4d T = endEffector.getPoseWorld();
	
	DHKeyframe newPoseFK = getIKSolver().createDHKeyframe();
	int i=0;
	// for all adjustable joints
	for( DHLink link : links ) {
		if(link.flags == LinkAdjust.NONE) continue;
		
		// use anglesB to get the hand matrix after a tiny adjustment on one joint.
		newPoseFK.set(keyframe);
		newPoseFK.fkValues[i]+=ANGLE_STEP_SIZE_DEGREES;
		setPoseFK(newPoseFK);
		// Tnew will be different from T because of the changes in setPoseFK().
		Matrix4d Tnew = endEffector.getPoseWorld();
		
		// use the finite difference in the two matrixes
		// aka the approximate the rate of change (aka the integral, aka the velocity)
		// in one column of the jacobian matrix at this position.
		Matrix4d dT = new Matrix4d();
		dT.sub(Tnew,T);
		dT.mul(1.0/Math.toRadians(ANGLE_STEP_SIZE_DEGREES));
		
		jacobian[i][0]=dT.m03;
		jacobian[i][1]=dT.m13;
		jacobian[i][2]=dT.m23;

		// find the rotation part
		// these initialT and initialTd were found in the comments on
		// https://robotacademy.net.au/masterclass/velocity-kinematics-in-3d/?lesson=346
		// and used to confirm that our skew-symmetric matrix match theirs.
		/*
		double[] initialT = {
				 0,  0   , 1   ,  0.5963,
				 0,  1   , 0   , -0.1501,
				-1,  0   , 0   , -0.01435,
				 0,  0   , 0   ,  1 };
		double[] initialTd = {
				 0, -0.01, 1   ,  0.5978,
				 0,  1   , 0.01, -0.1441,
				-1,  0   , 0   , -0.01435,
				 0,  0   , 0   ,  1 };
		T.set(initialT);
		Td.set(initialTd);
		dT.sub(Td,T);
		dT.mul(1.0/Math.toRadians(ANGLE_STEP_SIZE_DEGREES));//*/
		
		//Log.message("T="+T);
		//Log.message("Td="+Td);
		//Log.message("dT="+dT);
		Matrix3d T3 = new Matrix3d(
				T.m00,T.m01,T.m02,
				T.m10,T.m11,T.m12,
				T.m20,T.m21,T.m22);
		//Log.message("R="+R);
		Matrix3d dT3 = new Matrix3d(
				dT.m00,dT.m01,dT.m02,
				dT.m10,dT.m11,dT.m12,
				dT.m20,dT.m21,dT.m22);
		//Log.message("dT3="+dT3);
		Matrix3d skewSymmetric = new Matrix3d();
		
		T3.transpose();  // inverse of a rotation matrix is its transpose
		skewSymmetric.mul(dT3,T3);
		
		//Log.message("SS="+skewSymmetric);
		//[  0 -Wz  Wy]
		//[ Wz   0 -Wx]
		//[-Wy  Wx   0]
		
		jacobian[i][3]=skewSymmetric.m12;
		jacobian[i][4]=skewSymmetric.m20;
		jacobian[i][5]=skewSymmetric.m01;
		
		++i;
	}
	
	// undo our changes.
	setPoseFK(oldPoseFK);
	
	return jacobian;
}
 

public DHTool_Gripper() {
	super();
	setLetter("T");
	setName("Gripper");
	refreshPoseMatrix();
	
	gripperServoAngle=90;
	interpolatePoseT=1;
	startT=endT=gripperServoAngle;
	
	setModelFilename("/Sixi2/beerGripper/base.stl");
	setModelScale(0.1f);
	setModelOrigin(-1,0,4.15);
	setModelRotation(0,180,90);
	

	Matrix3d r = new Matrix3d();
	r.setIdentity();
	r.rotX(Math.toRadians(180));
	Matrix3d r2 = new Matrix3d();
	r2.setIdentity();
	r2.rotZ(Math.toRadians(90));
	r.mul(r2);
	this.setRotation(r);
	
	// 4 bars
	addChild(subComponents[0]=new DHLink());
	addChild(subComponents[1]=new DHLink());
	addChild(subComponents[2]=new DHLink());
	addChild(subComponents[3]=new DHLink());
	subComponents[0].setModelFilename("/Sixi2/beerGripper/linkage.stl");
	subComponents[0].setModelScale(0.1f);
	subComponents[1].set(subComponents[0]);
	subComponents[2].set(subComponents[0]);
	subComponents[3].set(subComponents[0]);
	subComponents[0].setPosition(new Vector3d(2.7/2, 0, 4.1));
	subComponents[1].setPosition(new Vector3d(1.1/2, 0, 5.9575));
	subComponents[2].setPosition(new Vector3d(-2.7/2, 0, 4.1));
	subComponents[3].setPosition(new Vector3d(-1.1/2, 0, 5.9575));
	
	// 2 finger tips
	addChild(subComponents[4]=new DHLink());
	subComponents[4].setModelFilename("/Sixi2/beerGripper/finger.stl");
	subComponents[4].setModelScale(0.1f);
	addChild(subComponents[5]=new DHLink());
	subComponents[5].set(subComponents[4]);
	
	wasGripping=false;
}