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

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

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

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

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

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

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

public void createDefaultWorld() {
		//addChild(new SkyBoxEntity());
		
		// adjust default camera
		RobotOverlord ro = (RobotOverlord)getRoot();
		ro.camera.setPosition(new Vector3d(40,-91,106));
		ro.camera.setPan(-16);
		ro.camera.setTilt(53);
		ro.camera.setZoom(100);
		ro.camera.update(0);
		
		// add some lights
    	LightEntity light;

		addChild(light = new LightEntity());
		light.setName("Light 1");
    	light.lightIndex=1;
    	light.setPosition(new Vector3d(60,-60,160));
    	light.setDiffuse(1,1,1,1);
    	light.setSpecular(0.5f, 0.5f, 0.5f, 1.0f);
    	light.attenuationLinear.set(0.0014);
    	light.attenuationQuadratic.set(7*1e-6);
    	light.setDirectional(true);
    	
		// add some collision bounds
		BoxEntity box;
		
		addChild(box = new BoxEntity());
		box.setName("Front wall");
		box.setSize(233.5,100,1);
		box.setPosition(new Vector3d(69.75,65,0));
		box.getMaterial().setDiffuseColor(0f/255f,169f/255f,255f/255f,1f);
		
		addChild(box = new BoxEntity());
		box.setName("Back wall");
		box.setSize(180,100,1);
		box.setPosition(new Vector3d(-47.5,-25.5,0));
		box.setRotation(new Vector3d(0, 0, Math.toRadians(-90)));
		box.getMaterial().setDiffuseColor(0f/255f,169f/255f,255f/255f,1f);

		ModelEntity table = new ModelEntity("/table.stl");
		addChild(table);
		table.setName("Table");
		table.setPosition(new Vector3d(0,0,-0.75));
		//box.setSize(160,1,110);
		//box.setPosition(new Vector3d(59.5,0,-2.5));
/*
		// adjust grid
		GridEntity grid = new GridEntity();
		addChild(grid);
		grid.width.set(140);
		grid.height.set(90);
		grid.setPosition(new Vector3d(60.0,0,-0.5));
*/
    	// add a sixi robot
		Sixi2 sixi2=new Sixi2();
		addChild(sixi2);
		//sixi2.setPosition(new Vector3d(78,-25,0));
		Matrix3d m=new Matrix3d();
		m.setIdentity();
		//m.rotZ(Math.toRadians(-90));
		sixi2.setRotation(m);
		
		TrayCabinet trayCabinet=new TrayCabinet();
		addChild(trayCabinet);
		trayCabinet.setPosition(new Vector3d(35,49.5,0));
		TrayCabinet trayCabinet2=new TrayCabinet();
		addChild(trayCabinet2);
		trayCabinet2.setPosition(new Vector3d(35,49.5,21.75));
	}
 

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