javax.vecmath.Matrix3d Java Examples

@Override
public Matrix3d getViewMatrix(int index) {
	Matrix3d m = new Matrix3d();
	switch (index) {
	case 0:  m.setIdentity(); // front
	break;
	case 1:  m.rotY(Math.PI/2); // left
	break;
	case 2:  m.rotY(Math.PI); // back
	break;
	case 3:  m.rotY(-Math.PI/2); // right
	break;
	case 4:  m.rotX(Math.PI/2); // top
	break;
	case 5:  m.rotX(-Math.PI/2); // bottom
	break;
	default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
	}
	return m;
}
 

/**
 * Convert a matrix to Euler rotations.  There are many valid solutions.
 * See also https://www.learnopencv.com/rotation-matrix-to-euler-angles/
 * @param mat the Matrix3d to convert.
 * @return a Vector3d resulting radian rotations.  One possible solution.
 */
public static Vector3d matrixToEuler(Matrix3d mat) {
	assert(isRotationMatrix(mat));
	
	double sy = Math.sqrt(mat.m00*mat.m00 + mat.m10*mat.m10);
	boolean singular = sy < 1e-6;
	double x,y,z;
	if(!singular) {
		x = Math.atan2( mat.m21,mat.m22);
		y = Math.atan2(-mat.m20,sy);
		z = Math.atan2( mat.m10,mat.m00);
	} else {
		x = Math.atan2(-mat.m12, mat.m11);
		y = Math.atan2(-mat.m20, sy);
		z = 0;
	}
	return new Vector3d(x,y,z);
}
 

public static Matrix4d getMatrixFromAlgebraic(String transfAlgebraic) {
	String[] parts = transfAlgebraic.toUpperCase().split(",");
	double[] xCoef = convertAlgebraicStrToCoefficients(parts[0].trim());
	double[] yCoef = convertAlgebraicStrToCoefficients(parts[1].trim());
	double[] zCoef = convertAlgebraicStrToCoefficients(parts[2].trim());

	Matrix4d mat = new Matrix4d();
	mat.setIdentity();
	mat.setRotation(new Matrix3d(xCoef[0],xCoef[1],xCoef[2],yCoef[0],yCoef[1],yCoef[2],zCoef[0],zCoef[1],zCoef[2]));
	mat.setTranslation(new Vector3d(xCoef[3],yCoef[3],zCoef[3]));
	return mat;
	//return new Matrix4d(xCoef[0],xCoef[1],xCoef[2],xCoef[3],
	//					yCoef[0],yCoef[1],yCoef[2],yCoef[3],
	//					zCoef[0],zCoef[1],zCoef[2],zCoef[3],
	//					0,0,0,1);
}
 

/**
 * Generic quadcopter constructor.
 *
 * @param world          world where to place the vehicle
 * @param modelName      filename of model to load, in .obj format
 * @param orientation    "x" or "+"
 * @param armLength      length of arm from center
 * @param rotorThrust    full thrust of one rotor
 * @param rotorTorque    torque at full thrust of one rotor
 * @param rotorTimeConst spin-up time of rotor
 * @param rotorsOffset   rotors positions offset from gravity center
 * @throws FileNotFoundException if .obj model file not found
 */
public Quadcopter(World world, String modelName, String orientation, double armLength, double rotorThrust,
                  double rotorTorque, double rotorTimeConst, Vector3d rotorsOffset) throws FileNotFoundException {
    super(world, modelName);
    rotorPositions[0] = new Vector3d(0.0, armLength, 0.0);
    rotorPositions[1] = new Vector3d(0.0, -armLength, 0.0);
    rotorPositions[2] = new Vector3d(armLength, 0.0, 0.0);
    rotorPositions[3] = new Vector3d(-armLength, 0.0, 0.0);
    if (orientation.equals("x") || orientation.equals("X")) {
        Matrix3d r = new Matrix3d();
        r.rotZ(-Math.PI / 4);
        for (int i = 0; i < rotorsNum; i++) {
            r.transform(rotorPositions[i]);
        }
    } else if (orientation.equals("+")) {
    } else {
        throw new RuntimeException("Unknown orientation: " + orientation);
    }
    for (int i = 0; i < rotors.length; i++) {
        rotorPositions[i].add(rotorsOffset);
        Rotor rotor = rotors[i];
        rotor.setFullThrust(rotorThrust);
        rotor.setFullTorque(i < 2 ? -rotorTorque : rotorTorque);
        rotor.setTimeConstant(rotorTimeConst);
    }
}
 

/**
 * 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;
}
 

@Override
public void update(long t) {
    this.position = baseObject.position;
    this.velocity = baseObject.velocity;
    this.acceleration = baseObject.acceleration;
    double yaw = Math.atan2(baseObject.rotation.getElement(1, 0), baseObject.rotation.getElement(0, 0));
    this.rotation.rotZ(yaw);
    if (pitchChannel >= 0 && baseObject instanceof AbstractVehicle) {
        // Control camera pitch
        List<Double> control = ((AbstractVehicle) baseObject).getControl();
        if (control.size() > pitchChannel) {
            Matrix3d r = new Matrix3d();
            r.rotY(control.get(4) * pitchScale);
            this.rotation.mul(r);
        }
    }
}
 

/**
 * 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;
}
 

@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;
}
 

@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;
}
 

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;
}
 

/**
 * Returns the vertices of an n-fold polygon of given radius and center
 * @return
 */
@Override
public Point3d[] getVertices() {
	Point3d[] polygon = new Point3d[2*n];
	Matrix3d m = new Matrix3d();

	Point3d center = new Point3d(0, 0, height/2);

	for (int i = 0; i < n; i++) {
		polygon[i] = new Point3d(0, circumscribedRadius, 0);
		m.rotZ(i*2*Math.PI/n);
		m.transform(polygon[i]);
		polygon[n+i] = new  Point3d(polygon[i]);
		polygon[i].sub(center);
		polygon[n+i].add(center);
	}

	return polygon;
}
 

/**
 * Returns the vertices of an n-fold polygon of given radius and center
 * @param n
 * @param radius
 * @param center
 * @return
 */
@Override
public  Point3d[] getVertices() {
	double x = getSideLengthFromCircumscribedRadius(circumscribedRadius)/2;
	double z = x/Math.sqrt(2);
	Point3d[] tetrahedron = new Point3d[4];
	tetrahedron[0] = new Point3d(-x,  0, -z);
	tetrahedron[1] = new Point3d( x,  0, -z);
	tetrahedron[2] = new Point3d( 0, -x,  z);
	tetrahedron[3] = new Point3d( 0,  x,  z);
	Point3d centroid = CalcPoint.centroid(tetrahedron);

	// rotate tetrahedron to align one vertex with the +z axis
	Matrix3d m = new Matrix3d();
	m.rotX(0.5 * TETRAHEDRAL_ANGLE);
	for (Point3d p: tetrahedron) {
		p.sub(centroid);
		m.transform(p);
	}
	return tetrahedron;
}
 

@Test
public void testEulerMatrix() {
	Vector3d v1 = new Vector3d();
	
	for(int i=0;i<1000;++i) {
		v1.x = Math.random() * Math.PI*2.0;
		v1.y = Math.random() * Math.PI*2.0;
		v1.z = Math.random() * Math.PI*2.0;
		
		Matrix3d a = MatrixHelper.eulerToMatrix(v1);
		Vector3d v2 = MatrixHelper.matrixToEuler(a);
		Matrix3d b = MatrixHelper.eulerToMatrix(v2);
		
		boolean test = b.epsilonEquals(a, 1e-6);
		assert(test);
		if(test==false) {
			System.out.println(i+"a="+a);
			System.out.println(i+"b="+b);
			b.sub(a);
			System.out.println(i+"d="+b);
		}
		org.junit.Assert.assertTrue(test);
	}
	System.out.println("testEulerMatrix() OK");
}
 

@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;
}
 

/**
 * Produce a Detector properties object populated with sensible default values given image shape.
 * It produces a detector normal to the beam and centred on the beam with square pixels of 0.1024mm and set 200mm
 * from the sample.
 * 
 * @param shape image shape
 */
public static DetectorProperties getDefaultDetectorProperties(int... shape) {
	int heightInPixels = shape[0];
	int widthInPixels = shape[1];
	
	// Set a few default values
	double pixelSizeX = 0.1024;
	double pixelSizeY = 0.1024;
	double distance = 200.00;
	
	// Create identity orientation
	Matrix3d identityMatrix = new Matrix3d();
	identityMatrix.setIdentity();

	// Create the detector origin vector based on the above
	Vector3d dOrigin = new Vector3d((widthInPixels - widthInPixels/2d) * pixelSizeX, (heightInPixels - heightInPixels/2d) * pixelSizeY, distance);

	return new DetectorProperties(dOrigin, heightInPixels, widthInPixels, pixelSizeX, pixelSizeY, identityMatrix);
}
 

/**
 * Reset entries that are less than or equal to 1 unit of least precision of
 * the matrix's scale
 * @param m
 */
private static void santise(Matrix3d m) {
	double scale = m.getScale();
	double min = Math.ulp(scale);
	for (int i = 0; i < 3; i++) {
		double t;
		t = Math.abs(m.getElement(i, 0));
		if (t > 0 && t <= min) {
			m.setElement(i, 0, 0);
		}
		t = Math.abs(m.getElement(i, 1));
		if (t > 0 && t <= min) {
			m.setElement(i, 1, 0);
		}
		t = Math.abs(m.getElement(i, 2));
		if (t > 0 && t <= min) {
			m.setElement(i, 2, 0);
		}
	}
}
 

/**
 * Returns the vertices of an n-fold polygon of given radius and center
 * @return
 */
public static Point3d[] getPolygonVertices(int n, double radius, Point3d center) {
	Point3d[] polygon = new Point3d[n];
	Matrix3d m = new Matrix3d();

	for (int i = 0; i < n; i++) {
		polygon[i] = new Point3d(0, radius, 0);
		m.rotZ(i*2*Math.PI/n);
		m.transform(polygon[i]);
		polygon[i].add(center);
	}
	return polygon;
}
 

/**
 * Generic hexacopter constructor.
 *
 * @param world          world where to place the vehicle
 * @param modelName      filename of model to load, in .obj format
 * @param orientation    "x" or "+"
 * @param armLength      length of arm from center
 * @param rotorThrust    full thrust of one rotor
 * @param rotorTorque    torque at full thrust of one rotor
 * @param rotorTimeConst spin-up time of rotor
 * @param rotorsOffset   rotors positions offset from gravity center
 * @throws FileNotFoundException if .obj model file not found
 */
public Hexacopter(World world, String modelName, String orientation, double armLength, double rotorThrust,
                  double rotorTorque, double rotorTimeConst, Vector3d rotorsOffset) throws FileNotFoundException {
    super(world, modelName);
    rotorPositions[0] = new Vector3d(armLength, 0.0, 0.0);
    rotorPositions[1] = new Vector3d(-armLength, 0.0, 0.0);
    rotorPositions[2] = new Vector3d(-armLength * Math.cos(Math.PI / 3), -armLength * Math.sin(Math.PI / 3), 0.0);
    rotorPositions[3] = new Vector3d(armLength * Math.cos(Math.PI / 3), armLength * Math.sin(Math.PI / 3), 0.0);
    rotorPositions[4] = new Vector3d(armLength * Math.cos(Math.PI / 3), -armLength * Math.sin(Math.PI / 3), 0.0);
    rotorPositions[5] = new Vector3d(-armLength * Math.cos(Math.PI / 3), armLength * Math.sin(Math.PI / 3), 0.0);
    if (orientation.equals("x") || orientation.equals("X")) {
        Matrix3d r = new Matrix3d();
        r.rotZ(Math.PI / 2);
        for (int i = 0; i < rotorsNum; i++) {
            r.transform(rotorPositions[i]);
        }
    } else if (orientation.equals("+")) {
    } else {
        throw new RuntimeException("Unknown orientation: " + orientation);
    }
    for (int i = 0; i < rotors.length; i++) {
        rotorPositions[i].add(rotorsOffset);
        Rotor rotor = rotors[i];
        rotor.setFullThrust(rotorThrust);
        rotor.setFullTorque((i == 1 || i == 3 || i == 4) ? -rotorTorque : rotorTorque);
        rotor.setTimeConstant(rotorTimeConst);
    }
}
 

/**
 * Creates a standard 3x3 rotation matrix for this transform and the given transform type.
 *
 * @param transformType
 * @return
 */
public Matrix3d createRotationMatrix(TransformType transformType) {
    double[] internalRotationMatrix = getInternalMatrix(transformType);
    return new Matrix3d(internalRotationMatrix[0], internalRotationMatrix[1],
        internalRotationMatrix[2], internalRotationMatrix[4], internalRotationMatrix[5],
        internalRotationMatrix[6], internalRotationMatrix[8], internalRotationMatrix[9],
        internalRotationMatrix[10]);
}
 

private static Matrix3d flipX() {
	Matrix3d rot = new Matrix3d();
	rot.m00 = 1;
	rot.m11 = -1;
	rot.m22 = -1;
	return rot;
}
 

/**
 * Given a rotation matrix calculates the rotation axis and angle for it.
 * The angle is calculated from the trace, the axis from the eigenvalue
 * decomposition.
 * If given matrix is improper rotation or identity matrix then
 * axis (0,0,0) and angle 0 are returned.
 * @param m
 * @return
 * @throws IllegalArgumentException if given matrix is not a rotation matrix (determinant not 1 or -1)
 */
public static AxisAngle4d getRotAxisAndAngle(Matrix3d m) {
	double determinant = m.determinant();

	if (!(Math.abs(determinant)-1.0<DELTA)) throw new IllegalArgumentException("Given matrix is not a rotation matrix");

	AxisAngle4d axisAndAngle = new AxisAngle4d(new Vector3d(0,0,0),0);

	double[] d = {m.m00,m.m10,m.m20,
			m.m01,m.m11,m.m21,
			m.m02,m.m12,m.m22};

	Matrix r = new Matrix(d,3);

	if (!deltaComp(r.det(), 1.0, DELTA)) {
		// improper rotation: we return axis 0,0,0 and angle 0
		return axisAndAngle;
	}

	EigenvalueDecomposition evd = new EigenvalueDecomposition(r);

	Matrix eval = evd.getD();
	if (deltaComp(eval.get(0, 0),1.0,DELTA) && deltaComp(eval.get(1, 1),1.0,DELTA) && deltaComp(eval.get(2, 2),1.0,DELTA)) {
		// the rotation is an identity: we return axis 0,0,0 and angle 0
		return axisAndAngle;
	}
	int indexOfEv1;
	for (indexOfEv1=0;indexOfEv1<3;indexOfEv1++) {
		if (deltaComp(eval.get(indexOfEv1, indexOfEv1),1,DELTA)) break;
	}
	Matrix evec = evd.getV();
	axisAndAngle.set(new Vector3d(evec.get(0,indexOfEv1), evec.get(1, indexOfEv1), evec.get(2, indexOfEv1)),
			Math.acos((eval.trace()-1.0)/2.0));

	return axisAndAngle;
}
 

/**
 * Quickly compute the rotation angle from a rotation matrix.
 * @param transform 3D rotation matrix
 * @return Angle, from 0 to PI
 */
public static double getAngle(Matrix3d transform) {
	// Calculate angle
	double c = (transform.m00 + transform.m11 + transform.m22 - 1)/2.0; //=cos(theta)
	// c is sometimes slightly out of the [-1,1] range due to numerical instabilities
	if( -1-1e-8 < c && c < -1 ) c = -1;
	if( 1+1e-8 > c && c > 1 ) c = 1;
	if( -1 > c || c > 1 ) {
		throw new IllegalArgumentException("Input matrix is not a valid rotation matrix.");
	}
	return Math.acos(c);
}
 

@Override
public Point3d[] getVertices() {
	Point3d[] icosahedron = new Point3d[12];
	// see http://answers.yahoo.com/question/index?qid=20080108041441AAJCjEu
	double c = circumscribedRadius * 1 / Math.sqrt(5);
	double s = 2 * c; // golden ratio
	double c1 = Math.sqrt((3-Math.sqrt(5))/8); // cos(2Pi/5)
	double s1 = Math.sqrt((5+Math.sqrt(5))/8); // sin(2Pi/5)
	double c2 = Math.sqrt((3+Math.sqrt(5))/8); // cos(Pi/5)
	double s2 = Math.sqrt((5-Math.sqrt(5))/8); // sin(Pi/5)

	icosahedron[0] = new Point3d(0, 0, circumscribedRadius);
	icosahedron[1] = new Point3d(s, 0, c);
	icosahedron[2] = new Point3d(s*c1, s*s1, c);
	icosahedron[3] = new Point3d(-s*c2, s*s2, c);
	icosahedron[4] = new Point3d(-s*c2, -s*s2, c);
	icosahedron[5] = new Point3d(s*c1, -s*s1, c);
	for (int i = 0; i < 6; i++) {
		icosahedron[i+6] = new Point3d(icosahedron[i]);
		icosahedron[i+6].negate();
	}

	Matrix3d m = new Matrix3d();
	m.rotZ(Math.PI/10);
	for (Point3d p: icosahedron) {
		m.transform(p);
	}

	return icosahedron;
}
 

@Override
public Vector3d getAcc() {
    Vector3d accBody = new Vector3d(object.getAcceleration());
    accBody.sub(object.getWorld().getEnvironment().getG());
    Matrix3d rot = new Matrix3d(object.getRotation());
    rot.transpose();
    rot.transform(accBody);
    return accBody;
}
 

private Matrix3d getRotationMatrix() {
	getRmsd();
	if (!transformationCalculated) {
		calcRotationMatrix();
		transformationCalculated = true;
	}
	return rotmat;
}
 

/**
 * Rotates this coordinate about the input vector through the input angle (radians - because we
 * usually use this internally)
 *
 * @param rotAxis The axis of rotation
 * @param angle The angle of rotation (in radians)
 */
public Vector3d rotate(final Vector3d rotAxis, final double angle) {
  final Vector3d thisVec = new Vector3d(ecfVector);
  final Vector3d axis = new Vector3d(rotAxis);
  axis.normalize();

  final Matrix3d trans = new Matrix3d();
  trans.set(new AxisAngle4d(axis, angle));

  trans.transform(thisVec);

  return thisVec;
}
 

/**
 * 
 * @param original
 */
public void restore(DetectorProperties original) {
	fire = false;
	setOrigin(new Vector3d(original.getOrigin()));
	setBeamVector(new Vector3d(original.getBeamVector()));
	setPx(original.getPx());
	setPy(original.getPy());
	setStartX(original.getStartX());
	setStartY(original.getStartY());
	setVPxSize(original.getVPxSize());
	setHPxSize(original.getHPxSize());
	fire = true;
	setOrientation(new Matrix3d(original.getOrientation()));
}
 

@Override
public Vector3d getMag() {
    Vector3d mag = new Vector3d(object.getWorld().getEnvironment().getMagField(object.getPosition()));
    Matrix3d rot = new Matrix3d(object.getRotation());
    rot.transpose();
    rot.transform(mag);
    return mag;
}
 

@Override
public Matrix3d getViewMatrix(int index) {
	Matrix3d m = new Matrix3d();
	switch (index) {
	case 0:  m.setIdentity(); // front vertex-centered
	break;
	case 1:  m.rotX(Math.PI); // back face-centered
	break;
	case 2: double angle = Math.PI - 0.5 * TETRAHEDRAL_ANGLE; // Side edge-centered
	m.rotX(angle);
	break;
	default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
	}
	return m;
}