Java Matrix3d类代码示例

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * Converts this Rotation into PogamutLocation.
 * 
 * @param order
 *            order of rotations should the method use
 * @return converted Rotation into PogamutLocation
 */
public PogamutLocation toLocation(Order order) {
	Matrix3d yaw = constructXYRot(getYaw() / 32767 * Math.PI);
	Matrix3d roll = constructYZRot(getRoll() / 32767 * Math.PI);
	Matrix3d pitch = constructXZRot(getPitch() / 32767 * Math.PI);

	PogamutLocation res = new PogamutLocation(1, 0, 0);

	switch (order) {
	case YAW_PITCH_ROLL:
		return res.mul(yaw).mul(pitch).mul(roll);
	case ROLL_PITCH_YAW:
		return res.mul(roll).mul(pitch).mul(yaw);
	case PITCH_YAW_ROLL:
		return res.mul(pitch).mul(yaw).mul(roll);
	case PITCH_ROLL_YAW:
		return res.mul(pitch).mul(roll).mul(yaw);
	case YAW_ROLL_PITCH:
		return res.mul(yaw).mul(roll).mul(pitch);
	case ROLL_YAW_PITCH:
		return res.mul(roll).mul(yaw).mul(pitch);
	}

	return null;
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
public AffineTransform3D(double[] coeficients) {
    matrix = new Matrix4d();  	
    if (coeficients.length==9) {
        matrix.set(new Matrix3d(coeficients));
    } else if (coeficients.length==12) {
        double[] extendedCoeficients = new double[16];
        for (int i=0; i<coeficients.length; ++i) {
            extendedCoeficients[i] = coeficients[i];
        }
        extendedCoeficients[12] = 0;
        extendedCoeficients[13] = 0;
        extendedCoeficients[14] = 0;
        extendedCoeficients[15] = 1;
        matrix.set(extendedCoeficients);
    }
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * Converts this Rotation into Location.
 * 
 * @param order
 *            order of rotations should the method use
 * @return converted Rotation into Location
 */
public Location toLocation(Order order) {
	Matrix3d yaw = constructXYRot(getYaw() / 32767 * Math.PI);
	Matrix3d roll = constructYZRot(getRoll() / 32767 * Math.PI);
	Matrix3d pitch = constructXZRot(getPitch() / 32767 * Math.PI);

	Location res = new Location(1, 0, 0);

	switch (order) {
	case YAW_PITCH_ROLL:
		return res.mul(yaw).mul(pitch).mul(roll);
	case ROLL_PITCH_YAW:
		return res.mul(roll).mul(pitch).mul(yaw);
	case PITCH_YAW_ROLL:
		return res.mul(pitch).mul(yaw).mul(roll);
	case PITCH_ROLL_YAW:
		return res.mul(pitch).mul(roll).mul(yaw);
	case YAW_ROLL_PITCH:
		return res.mul(yaw).mul(roll).mul(pitch);
	case ROLL_YAW_PITCH:
		return res.mul(roll).mul(yaw).mul(pitch);
	}

	return null;
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * Dodges from the specified location.
 * 
 * @param inFrontOfTheBot Location to dodge from.
 * @param bDouble Whether to perform double dodge.
 */
public void dodgeBack(ILocated inFrontOfTheBot, boolean bDouble) {
    ILocated bot = self.getLocation();
    Location direction = new Location(bot.getLocation().x - inFrontOfTheBot.getLocation().x, bot.getLocation().y - inFrontOfTheBot.getLocation().y, 0);
    direction = direction.getNormalized();

    double alpha = Math.acos(self.getRotation().toLocation().getNormalized().dot(direction.getNormalized()));

    double orientation = self.getRotation().toLocation().cross(new Location(0, 0, 1)).dot(direction);
    if (orientation > 0) {
        alpha = -alpha;
    }

    Matrix3d rot = Rotation.constructXYRot(alpha);
    direction = new Location(1, 0, 0).mul(rot);

    this.act.act(new Dodge().setDirection(direction).setDouble(bDouble));
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * Dodges towards the specified location.
 * 
 * @param inFrontOfTheBot Location to dodge to.
 * @param bDouble Whether to perform double dodge.
 */
public void dodgeTo(ILocated inFrontOfTheBot, boolean bDouble) {
    ILocated bot = self.getLocation();
    Location direction = new Location(inFrontOfTheBot.getLocation().x - bot.getLocation().x, inFrontOfTheBot.getLocation().y - bot.getLocation().y, 0);
    direction = direction.getNormalized();

    double alpha = Math.acos(self.getRotation().toLocation().getNormalized().dot(direction.getNormalized()));

    double orientation = self.getRotation().toLocation().cross(new Location(0, 0, 1)).dot(direction);
    if (orientation > 0) {
        alpha = -alpha;
    }

    Matrix3d rot = Rotation.constructXYRot(alpha);
    direction = new Location(1, 0, 0).mul(rot);

    this.act.act(new Dodge().setDirection(direction).setDouble(bDouble));
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * Rotate an {@link AxisAlignedBB} by the specified rotation matrix.
 *
 * @param axisAlignedBB  The AABB
 * @param rotationMatrix The rotation matrix
 * @param forcePositive  If true, set each coordinate of the rotated AABB to it absolute value
 * @return The rotated AABB
 */
public static AxisAlignedBB rotateAABB(AxisAlignedBB axisAlignedBB, Matrix3d rotationMatrix, boolean forcePositive) {
    // Extract the minimum and maximum coordinates of the AABB into vectors
    final Vector3d minCoords = new Vector3d(axisAlignedBB.minX, axisAlignedBB.minY, axisAlignedBB.minZ);
    final Vector3d maxCoords = new Vector3d(axisAlignedBB.maxX, axisAlignedBB.maxY, axisAlignedBB.maxZ);

    // Rotate the vectors in-place
    rotationMatrix.transform(minCoords);
    rotationMatrix.transform(maxCoords);

    if (forcePositive) {
        // Get the absolute value of the coordinates
        minCoords.absolute();
        maxCoords.absolute();
    }

    // Return an AABB with the new coordinates
    return new AxisAlignedBB(minCoords.getX(), minCoords.getY(), minCoords.getZ(), maxCoords.getX(), maxCoords.getY(), maxCoords.getZ());
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * Getter for the rotation part of the complete transform as yaw, pitch and
 * roll angles.
 * 
 * @return The yaw, pitch and roll angles as Java3D Vecmath {@link Vector3d}
 */
public Vector3d getRotationYPR() {

	Matrix3d rot = getRotationMatrix();

	// this code is taken from buttel btMatrix3x3 getEulerYPR().
	// http://bulletphysics.org/Bullet/BulletFull/btMatrix3x3_8h_source.html
	// first use the normal calculus
	double yawOut = Math.atan2(rot.m10, rot.m00);
	double pitchOut = Math.asin(-rot.m20);
	double rollOut = Math.atan2(rot.m21, rot.m22);

	// on pitch = +/-HalfPI
	if (Math.abs(pitchOut) == Math.PI / 2.0) {
		if (yawOut > 0)
			yawOut -= Math.PI;
		else
			yawOut += Math.PI;
		if (pitchOut > 0)
			pitchOut -= Math.PI;
		else
			pitchOut += Math.PI;
	}

	return new Vector3d(yawOut, pitchOut, rollOut);
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
public static PointD addRotAroundAxis(double x, double y, double z, double axis) {

        // May be faster to rewrite with your own to reduce the number of calculations

        Matrix3d rotationMatrix = new Matrix3d();
        rotationMatrix.rotZ(z); // Add z rotation
        Matrix3d yRot = new Matrix3d();
        yRot.rotY(y);
        Matrix3d xRot = new Matrix3d();
        xRot.rotX(x);

        Matrix3d axisRot = new Matrix3d();
        axisRot.rotY(axis);

        rotationMatrix.mul(yRot);
        rotationMatrix.mul(xRot);
        rotationMatrix.mul(axisRot);

        return new PointD(Math.atan2(rotationMatrix.m21, rotationMatrix.m22),
                Math.atan2(-rotationMatrix.m20,Math.sqrt(rotationMatrix.m21 * rotationMatrix.m21 + rotationMatrix.m22 * rotationMatrix.m22)),
                Math.atan2(rotationMatrix.m10, rotationMatrix.m00));
    }
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * 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);
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * 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);
		}
	}
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
void showMatch(GFace face1, GFace face2, int matchIndex, GSolid solid) {
    matchLabels1 = new String[3];
    matchLabels2 = new String[3];
    int sc = face1.sideCount();
    boolean flipFace2 = face1.getOrientation(solid1, solid1.getGCenter())
        == face2.getOrientation(solid2, solid2.getGCenter());
    for (int i = 0; i < 3; i++) {
        matchLabels1[i] = face1.labelAt((matchIndex + i) % sc);
        if (flipFace2)
            matchLabels2[i] = face2.labelAt((sc - i) % sc);
        else
            matchLabels2[i] = face2.labelAt(i);
    }
    GDocumentHandler documentHandler = GDocumentHandler.getInstance();
    GFigure figure1 = document.getFigure(figure1Name);
    GFigure jointFigure = documentHandler.newFigure(solid);
    jointFigure.setTransparent(figure1.isTransparent());
    jointFigure.setLabelled(figure1.isLabelled());
    Matrix3d attitude = new Matrix3d(figure1.getCamera().getAttitude());
    jointFigure.getCamera().setAttitude(attitude);
    Color baseColor = figure1.getBaseColor();
    jointFigure.setBaseColor(new Color(baseColor.getRGB()));
    jointFigureName = jointFigure.getName();
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
void showMatch(GFace face1, GFace face2, int matchIndex, GSolid solid) {
    matchLabels1 = new String[3];
    matchLabels2 = new String[3];
    int sc = face1.sideCount();
    boolean flipFace2 = face1.getOrientation(solid1, solid1.getGCenter())
        == face2.getOrientation(solid2, solid2.getGCenter());
    for (int i = 0; i < 3; i++) {
        matchLabels1[i] = face1.labelAt((matchIndex + i) % sc);
        if (flipFace2) {
            matchLabels2[i] = face2.labelAt((sc - i) % sc);
        }
        else {
            matchLabels2[i] = face2.labelAt(i);
        }
    }
    GDocumentHandler documentHandler = GDocumentHandler.getInstance();
    GFigure figure1 = document.getFigure(figure1Name);
    GFigure jointFigure = documentHandler.newFigure(solid);
    jointFigure.setTransparent(figure1.isTransparent());
    jointFigure.setLabelled(figure1.isLabelled());
    Matrix3d attitude = new Matrix3d(figure1.getCamera().getAttitude());
    jointFigure.getCamera().setAttitude(attitude);
    Color baseColor = figure1.getBaseColor();
    jointFigure.setBaseColor(new Color(baseColor.getRGB()));
    jointFigureName = jointFigure.getName();
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
public void paintSphericalVanishingPoints(Graphics g) {
    final double EPS = 0.1;
    final int R = 1;
    g.setColor(Constants.Vanish_Color);
    Matrix3d RZ = new Matrix3d();
    Matrix3d RY = new Matrix3d();
    for (double rz = 0; rz < 2 * Math.PI; rz += EPS) {
        RZ.rotZ(rz);
        for (double ry = -Math.PI / 2; ry < Math.PI / 2; ry += EPS) {
            RY.rotY(ry);
            Vector3d v = new Vector3d(1, 0, 0);
            RY.transform(v);
            RZ.transform(v);
            Point2d vp = projection.vanishingPoint(v);
            if (vp != null) {
                g.fillOval((int) vp.x - R, (int) vp.y - R, 2 * R, 2 * R);
            }
        }
    }
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
public Projection getProjection() {
    double fx = focalLength * resolution.x / sensorSize.x;
    double fy = focalLength * resolution.y / sensorSize.y;
    Matrix3d K = new Matrix3d(fx, 0, center.x, 0, fy, center.y, 0, 0, 1);
    //K.setIdentity();

    Matrix3d R = new Matrix3d(this.rotation);
    R.transpose();
    VirtualCamera.logFrame().log(R.toString());

    Vector3d T = new Vector3d(this.position);
    T.scale(-1);
    R.transform(T);

    projection.set(new Projection(K, R, T));
    return this.projection;
}
 

import javax.vecmath.Matrix3d; //导入依赖的package包/类
/**
 * Creates a new instance of Translation.
 * 
 * @param center
 * @param xAngle
 * @param yAngle
 * @param zAngle
 */
public Rotation(Point3d center, double xAngle, double yAngle, double zAngle) {
	super();
	centerOfRotation = center;
	double x = Math.toRadians(xAngle);
	double y = Math.toRadians(yAngle);
	double z = Math.toRadians(zAngle);
	Matrix3d xrot = new Matrix3d();
	xrot.rotX(x);
	Matrix3d yrot = new Matrix3d();
	yrot.rotY(y);
	Matrix3d zrot = new Matrix3d();
	zrot.rotZ(z);
	yrot.mul(zrot);
	xrot.mul(yrot);
	rotation = xrot;
	toOrigin = new Translation(-centerOfRotation.x, -centerOfRotation.y,
			-centerOfRotation.z);
	toCenter = new Translation(centerOfRotation.x, centerOfRotation.y,
			centerOfRotation.z);
}