Java 类javax.vecmath.Matrix4d 实例源码

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

public AffineTransform3D(
    double xx, double yx, double zx, double tx,
    double xy, double yy, double zy, double ty,
    double xz, double yz, double zz, double tz
) {
    matrix = new Matrix4d();
    matrix.m00 = xx;
    matrix.m01 = yx;
    matrix.m02 = zx;
    matrix.m03 = tx;
    matrix.m10 = xy;
    matrix.m11 = yy;
    matrix.m12 = zy;
    matrix.m13 = ty;
    matrix.m20 = xz;
    matrix.m21 = yz;
    matrix.m22 = zz;
    matrix.m23 = tz;
    matrix.m30 = 0;
    matrix.m31 = 0;
    matrix.m32 = 0;
    matrix.m33 = 1;
}

public static Prof buildProfile( Line3d oLine, Point3d cen ) {

    Matrix4d to2D = new Matrix4d();
    Vector3d c2 = oLine.dir(), c3 = new Vector3d();

    c2.normalize();

    c3.cross( c2, UP );

    to2D.setIdentity();
    to2D.setRow( 0, c3.x, c3.y, c3.z, 0 );
    to2D.setRow( 1, UP.x, UP.y, UP.z, 0 );
    to2D.setRow( 2, c2.x, c2.y, c2.z, 0 );

    {
        Point3d start = new Point3d( cen.x, 0, cen.z );
        to2D.transform( start );
        to2D.m03 = -start.x;
        to2D.m13 = -start.y;
        to2D.m23 = -start.z;
        to2D.m33 = 1;
    }

    Prof monotonic = new Prof(to2D, c2);
    return monotonic;
}

private boolean inBounds( Matrix4d mini, List<double[]> bounds ) {

    // mini matrix is in mini-mesh format: a translation from a 255^3 cube in the first quadrant
    // trans.offset is a transform from that space, into jme rendered space (cartesian in meters, around the origin)

    Matrix4d m = new Matrix4d();
    m.mul( Jme3z.fromMatrix ( trans.offset ), mini );

    for (Point2d p : Arrays.stream( cubeCorners ).map( c -> { 
            Point3d tmp = new Point3d();
            m.transform( c, tmp );
            return new Point2d(tmp.x, tmp.z);
        }).collect( Collectors.toList() ) ) { 

            for (double[] bound : bounds) {
                 if ( 
                         bound[0] < p.x && bound[1] > p.x &&
                         bound[2] < p.y && bound[3] > p.y )
                     return true;
            }
    }

    return false;
}

@Override
public void writeTo(VirtualInstanceCollection virtualInstanceCollection, Class<?> type, Type genericType, Annotation[] annotations, MediaType mediaType, MultivaluedMap<String, Object> httpHeaders, OutputStream entityStream) throws UnsupportedEncodingException {
    String charSet = "UTF-8";
    JsonGenerator jg = Json.createGenerator(new OutputStreamWriter(entityStream, charSet));
    jg.writeStartArray();

    Matrix4d gM = new Matrix4d();
    gM.setIdentity();

    PartLink virtualRootPartLink = getVirtualRootPartLink(virtualInstanceCollection);
    List<PartLink> path = new ArrayList<>();
    path.add(virtualRootPartLink);
    InstanceBodyWriterTools.generateInstanceStreamWithGlobalMatrix(productService, path, gM, virtualInstanceCollection, new ArrayList<>(), jg);
    jg.writeEnd();
    jg.flush();
}

static Matrix4d combineTransformation(Matrix4d matrix, Vector3d translation, Vector3d rotation) {
    Matrix4d gM = new Matrix4d(matrix);
    Matrix4d m = new Matrix4d();

    m.setIdentity();
    m.setTranslation(translation);
    gM.mul(m);

    m.setIdentity();
    m.rotZ(rotation.z);
    gM.mul(m);

    m.setIdentity();
    m.rotY(rotation.y);
    gM.mul(m);

    m.setIdentity();
    m.rotX(rotation.x);
    gM.mul(m);

    return gM;
}

private static void writeLeaf(List<PartLink> currentPath, List<Integer> copyInstanceIds, PartIteration partI, Matrix4d combinedMatrix, JsonGenerator jg) {
    String partIterationId = partI.toString();
    List<InstanceAttributeDTO> attributes = new ArrayList<>();
    for (InstanceAttribute attr : partI.getInstanceAttributes()) {
        attributes.add(mapper.map(attr, InstanceAttributeDTO.class));
    }

    jg.writeStartObject();
    jg.write("id", Tools.getPathInstanceAsString(currentPath, copyInstanceIds));
    jg.write("partIterationId", partIterationId);
    jg.write("path", Tools.getPathAsString(currentPath));

    writeMatrix(combinedMatrix, jg);
    writeGeometries(partI.getSortedGeometries(), jg);
    writeAttributes(attributes, jg);

    jg.writeEnd();
    jg.flush();
}

public Matrix4 set(Matrix4d mat) {

        m00 = mat.m00;
        m01 = mat.m01;
        m02 = mat.m02;
        m03 = mat.m03;
        m10 = mat.m10;
        m11 = mat.m11;
        m12 = mat.m12;
        m13 = mat.m13;
        m20 = mat.m20;
        m21 = mat.m21;
        m22 = mat.m22;
        m23 = mat.m23;
        m30 = mat.m30;
        m31 = mat.m31;
        m32 = mat.m32;
        m33 = mat.m33;

        return this;
    }

/**
     * Multiply this transform by t1. This transform will be update
     * and the result returned
     * 
     * @param transform
     * @return this
     */
    public CellTransform mul(CellTransform in) {
        // This does not work when scale!=1
//        this.scale *= in.scale;
//        this.translation.addLocal(rotation.mult(in.translation).multLocal(in.scale));
//        this.rotation.multLocal(in.rotation);

        // Correctly calculate the multiplication.
        Quat4d q = new Quat4d(rotation.x, rotation.y, rotation.z, rotation.w);
        Vector3d t = new Vector3d(translation.x, translation.y, translation.z);
        Matrix4d m = new Matrix4d(q,t,scale);

        Quat4d q1 = new Quat4d(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w);
        Vector3d t1 = new Vector3d(in.translation.x, in.translation.y, in.translation.z);
        Matrix4d m1 = new Matrix4d(q1,t1,in.scale);

        m.mul(m1);

        m.get(q);
        m.get(t);
        scale = (float)m.getScale();
        rotation.set((float)q.x, (float)q.y, (float)q.z, (float)q.w);
        translation.set((float)t.x, (float)t.y, (float)t.z);

        return this;
    }

static public DoubleBuffer getDoubleBuffer(final Matrix4d matrix) {
    final DoubleBuffer result = DoubleBuffer.allocate(16);
    result.put(0, matrix.m00);
    result.put(1, matrix.m01);
    result.put(2, matrix.m02);
    result.put(3, matrix.m03);
    result.put(4, matrix.m10);
    result.put(5, matrix.m11);
    result.put(6, matrix.m12);
    result.put(7, matrix.m13);
    result.put(8, matrix.m20);
    result.put(9, matrix.m21);
    result.put(10, matrix.m22);
    result.put(11, matrix.m23);
    result.put(12, matrix.m30);
    result.put(13, matrix.m31);
    result.put(14, matrix.m32);
    result.put(15, matrix.m33);
    return result;
}

static public double[] getDoubleArray(final Matrix4d matrix) {
    final double[] result = new double[16];
    result[0] = matrix.m00;
    result[1] = matrix.m01;
    result[2] = matrix.m02;
    result[3] = matrix.m03;
    result[4] = matrix.m10;
    result[5] = matrix.m11;
    result[6] = matrix.m12;
    result[7] = matrix.m13;
    result[8] = matrix.m20;
    result[9] = matrix.m21;
    result[10] = matrix.m22;
    result[11] = matrix.m23;
    result[12] = matrix.m30;
    result[13] = matrix.m31;
    result[14] = matrix.m32;
    result[15] = matrix.m33;
    return result;
}

/**
 * Make rotate input vector with Yaw(Z),Pitch(Y) and Roll(X) unreal rotation
 * values in the YXZ UT Editor coordinate system.
 * +00576.000000,+01088.000000,+00192.000000 ->
 * -00192.000000,+00064.000000,+00192.000000
 * 
 * @param v
 * @param pitch
 *            Pitch in Unreal Value (65536 u.v. = 360°)
 * @param yaw
 *            Yaw
 * @param roll
 * @return
 */
public static Vector3d rotate(Vector3d v, double pitch, double yaw, double roll) {

    pitch = UE12AngleToDegree(pitch);
    yaw = UE12AngleToDegree(yaw);
    roll = UE12AngleToDegree(roll);

    // TODO only divide by 360 if rotation values comes from Unreal Engine
    // 1/2
    double rot_x = ((roll) / 360D) * 2D * Math.PI; // Roll=Axis X with
                                                    // Unreal Editor
    double rot_y = (((pitch)) / 360D) * 2D * Math.PI; // Pitch=Axis Y with
                                                        // Unreal Editor
    double rot_z = ((yaw) / 360D) * 2D * Math.PI; // Yaw=Axis Z with Unreal
                                                    // Editor

    double tmp[] = new double[] { v.x, v.y, v.z, 1D };
    Matrix4d m4d = getGlobalRotationMatrix(rot_x, rot_y, rot_z);

    tmp = getRot(tmp, m4d);
    v.x = tmp[0];
    v.y = tmp[1];
    v.z = tmp[2];

    return v;
}

/**
 * 
 * @param rot_x
 * @param rot_y
 * @param rot_z
 * @return
 */
private static Matrix4d getGlobalRotationMatrix(double rot_x, double rot_y, double rot_z) {
    Matrix4d m4d;

    // Checked
    Matrix4d m4d_x = new Matrix4d(1D, 0D, 0D, 0D, 0D, Math.cos(rot_x), -Math.sin(rot_x), 0D, 0D, Math.sin(rot_x), Math.cos(rot_x), 0D, 0D, 0D, 0D, 1D);

    // Checked
    Matrix4d m4d_y = new Matrix4d(Math.cos(rot_y), 0D, Math.sin(rot_y), 0D, 0D, 1D, 0D, 0D, -Math.sin(rot_y), 0, Math.cos(rot_y), 0D, 0D, 0D, 0D, 1D);

    // Checked
    Matrix4d m4d_z = new Matrix4d(Math.cos(rot_z), Math.sin(rot_z), 0D, 0D, -Math.sin(rot_z), Math.cos(rot_z), 0D, 0D, 0D, 0D, 1D, 0D, 0D, 0D, 0D, 1D);
    m4d_x = updateMatrix(m4d_x);
    m4d_y = updateMatrix(m4d_y);
    m4d_z = updateMatrix(m4d_z);

    m4d = m4d_x;
    m4d.mul(m4d_y);
    m4d.mul(m4d_z);

    return m4d;
}

private static double[] getRot(double[] d2, Matrix4d m4d) {
    double d[] = new double[] { d2[0], d2[1], d2[2], 1D };

    double dx = m4d.m00 * d[0] + m4d.m10 * d[1] + m4d.m20 * d[2] + m4d.m30 * d[3];

    double dy = m4d.m01 * d[0] + m4d.m11 * d[1] + m4d.m21 * d[2] + m4d.m31 * d[3];
    double dz = m4d.m02 * d[0] + m4d.m12 * d[1] + m4d.m22 * d[2] + m4d.m32 * d[3];

    if (Math.abs(dx) < 0.00001D) {
        dx = 0D;
    }
    if (Math.abs(dy) < 0.00001D) {
        dy = 0D;
    }
    if (Math.abs(dz) < 0.00001D) {
        dz = 0D;
    }

    return new double[] { dx, dy, dz };
}

public TrackballRenderingViewer( CameraController.Viewer delegate )
{
    this .delegate = delegate;

    this .translation = new Vector3d();
    Matrix4d matrix = new Matrix4d();
    Camera defaultCamera = new Camera();
    defaultCamera .setMagnification( 1.0f );
    defaultCamera .getViewTransform( matrix, 0d );
    matrix .get( translation ); // save the default translation to apply on every update below

    // set the perspective view just once
    double near = defaultCamera .getNearClipDistance();
       double far = defaultCamera .getFarClipDistance();
       double fov = defaultCamera .getFieldOfView();
    this .delegate .setPerspective( fov, 1.0d, near, far );
}

/**
 * @param v[]
 * @param colors
 * @param lights
 * @param model
 */
public Java2dExporter( Camera view, Colors colors, Lights lights, RenderedModel model )
{
    super( view, colors, lights, model );

    this .view = view;
    Matrix4d viewMatrix = new Matrix4d();
    this .view .getViewTransform( viewMatrix, 0d );
    this .viewTransform = new Transform3D( viewMatrix );

    for ( int i = 0; i < lightDirs.length; i++ ) {
        lightDirs[ i ] = new Vector3f();
        // the next line fills in the direction, as well as returning the color... bad style!
        lightColors[ i ] = new Color( mLights .getDirectionalLight( i, lightDirs[ i ] ) .getRGB() );
        // the lights stay fixed relative to the viewpoint, so we must not apply the view transform
        lightDirs[ i ] .normalize();
        lightDirs[ i ] .negate();
    }
    ambientLight = new Color( mLights .getAmbientColor() .getRGB() );

}

/**
   * Given a point in world coordinates, adjust it in place to be in
   * screen coordinates (after projection).
   * 
   * @param point
   */
  public void mapWorldToScreen( Camera view, Point3f point )
  {
      Matrix4d viewMatrix = new Matrix4d();
      this .view .getViewTransform( viewMatrix, 0d );
      Transform3D viewTrans = new Transform3D( viewMatrix );
      viewTrans .transform( point );
      // point is now in view coordinates
      Vector4f p4 = new Vector4f( point.x, point.y, point.z, 1f );

      Transform3D eyeTrans = new Transform3D();
      if ( ! view .isPerspective() ) {
    double edge = view .getWidth() / 2;
    eyeTrans .ortho( -edge, edge, -edge, edge, view .getNearClipDistance(), view .getFarClipDistance() );
}
else
    eyeTrans .perspective( view .getFieldOfView(), 1.0d, view .getNearClipDistance(), view .getFarClipDistance() );
    // TODO - make aspect ratio track the screen window shape

      eyeTrans .transform( p4 );
      point .project( new Point4f( p4 ) );
  }

private void updateViewersTransformation()
{
       if ( mViewers .size() == 0 )
           return;
    Matrix4d trans = new Matrix4d();

    model .getViewTransform( trans, 0d );
       trans .invert();
       for ( int i = 0; i < mViewers .size(); i++ )
           mViewers .get( i ) .setViewTransformation( trans, Viewer .MONOCULAR );

       model .getStereoViewTransform( trans, Viewer .LEFT_EYE );
       trans .invert();
       for ( int i = 0; i < mViewers .size(); i++ )
           mViewers .get( i ) .setViewTransformation( trans, Viewer .LEFT_EYE );

       model .getStereoViewTransform( trans, Viewer .RIGHT_EYE );
       trans .invert();
       for ( int i = 0; i < mViewers .size(); i++ )
           mViewers .get( i ) .setViewTransformation( trans, Viewer .RIGHT_EYE );
}

/**
 * Applies a mirror operation to this transform.
 *
 * @param plane the plane that defines the mirror operation
 *
 * @return this transform
 */
public Transform mirror(Plane plane) {

    System.err.println("WARNING: I'm too dumb to implement the mirror() operation correctly. Please fix me!");

    double nx = plane.normal.x;
    double ny = plane.normal.y;
    double nz = plane.normal.z;
    double w = plane.dist;
    double elemenents[] = {
        (1.0 - 2.0 * nx * nx), (-2.0 * ny * nx), (-2.0 * nz * nx), 0,
        (-2.0 * nx * ny), (1.0 - 2.0 * ny * ny), (-2.0 * nz * ny), 0,
        (-2.0 * nx * nz), (-2.0 * ny * nz), (1.0 - 2.0 * nz * nz), 0,
        (-2.0 * nx * w), (-2.0 * ny * w), (-2.0 * nz * w), 1
    };
    m.mul(new Matrix4d(elemenents));
    return this;
}

/**
 * Get the mapping from view to world coordinates
 * @param trans
 */
public void getViewTransform( Matrix4d matrix, double angle )
{
    Point3d eyePoint = new Point3d( mLookAtPoint );
    Vector3d dir = new Vector3d( mLookDirection );
    if ( angle != 0d ) {
        double[] rotMat = new double[16];
        AxisAngle4d rotAA = new AxisAngle4d( mUpDirection, angle );
        set( rotAA, rotMat );
        transform( rotMat, dir );
    }
    dir .scale( -mDistance );
    eyePoint .add( dir );

    double[] mat = new double[16];
    lookAt( mat, eyePoint, mLookAtPoint, mUpDirection );
    matrix .set( mat );
}

protected void setDefaults( CPUAlgebraicSurfaceRenderer asr )
    {
        asr.getCamera().loadProperties( defaults, "camera_", "" );
        Util.setOptimalCameraDistance( asr.getCamera() );

        for( int i = 0; i < AlgebraicSurfaceRenderer.MAX_LIGHTS; i++ )
        {
            asr.getLightSource( i ).setStatus(LightSource.Status.OFF);
            asr.getLightSource( i ).loadProperties( defaults, "light_", "_" + i );
        }
        asr.setBackgroundColor( BasicIO.fromColor3fString( defaults.getProperty( "background_color" ) ) );

//        identity.setIdentity();
//        asr.setTransform( BasicIO.fromMatrix4dString( defaults.getProperty( "rotation_matrix" ) ) );
        Matrix4d scaleMatrix = new Matrix4d();
        scaleMatrix.setIdentity();
        scaleMatrix.setScale( 1.0 / Double.parseDouble( defaults.getProperty( "scale_factor" ) ) );
        asr.setSurfaceTransform( scaleMatrix );

        asr.setAntiAliasingMode( AntiAliasingMode.ADAPTIVE_SUPERSAMPLING );
        asr.setAntiAliasingPattern( AntiAliasingPattern.RG_2x2 );        
    }

public void setMatrix(Matrix4d p) {
    this.spinner11.setValue(p.m00);
    this.spinner12.setValue(p.m01);
    this.spinner13.setValue(p.m02);
    this.spinner14.setValue(p.m03);
    //
    this.spinner21.setValue(p.m10);
    this.spinner22.setValue(p.m11);
    this.spinner23.setValue(p.m12);
    this.spinner24.setValue(p.m13);
    //
    this.spinner31.setValue(p.m20);
    this.spinner32.setValue(p.m21);
    this.spinner33.setValue(p.m22);
    this.spinner34.setValue(p.m23);
}

public TrackballRenderingViewer( CameraController.Viewer delegate )
{
    this .delegate = delegate;

    this .translation = new Vector3d();
    Matrix4d matrix = new Matrix4d();
    Camera defaultCamera = new Camera();
    defaultCamera .setMagnification( 1.0f );
    defaultCamera .getViewTransform( matrix, 0d );
    matrix .get( translation ); // save the default translation to apply on every update below

    // set the perspective view just once
    double near = defaultCamera .getNearClipDistance();
       double far = defaultCamera .getFarClipDistance();
       double fov = defaultCamera .getFieldOfView();
    this .delegate .setPerspective( fov, 1.0d, near, far );
}

/**
 * @param v[]
 * @param colors
 * @param lights
 * @param model
 */
public Java2dExporter( Camera view, Colors colors, Lights lights, RenderedModel model )
{
    super( view, colors, lights, model );

    this .view = view;
    Matrix4d viewMatrix = new Matrix4d();
    this .view .getViewTransform( viewMatrix, 0d );
    this .viewTransform = new Transform3D( viewMatrix );

    for ( int i = 0; i < lightDirs.length; i++ ) {
        lightDirs[ i ] = new Vector3f();
        // the next line fills in the direction, as well as returning the color... bad style!
        lightColors[ i ] = new Color( mLights .getDirectionalLight( i, lightDirs[ i ] ) .getRGB() );
        // the lights stay fixed relative to the viewpoint, so we must not apply the view transform
        lightDirs[ i ] .normalize();
        lightDirs[ i ] .negate();
    }
    ambientLight = new Color( mLights .getAmbientColor() .getRGB() );

}

/**
   * Given a point in world coordinates, adjust it in place to be in
   * screen coordinates (after projection).
   * 
   * @param point
   */
  public void mapWorldToScreen( Camera view, Point3f point )
  {
      Matrix4d viewMatrix = new Matrix4d();
      this .view .getViewTransform( viewMatrix, 0d );
      Transform3D viewTrans = new Transform3D( viewMatrix );
      viewTrans .transform( point );
      // point is now in view coordinates
      Vector4f p4 = new Vector4f( point.x, point.y, point.z, 1f );

      Transform3D eyeTrans = new Transform3D();
      if ( ! view .isPerspective() ) {
    double edge = view .getWidth() / 2;
    eyeTrans .ortho( -edge, edge, -edge, edge, view .getNearClipDistance(), view .getFarClipDistance() );
}
else
    eyeTrans .perspective( view .getFieldOfView(), 1.0d, view .getNearClipDistance(), view .getFarClipDistance() );
    // TODO - make aspect ratio track the screen window shape

      eyeTrans .transform( p4 );
      point .project( new Point4f( p4 ) );
  }

private void updateViewersTransformation()
{
       if ( mViewers .size() == 0 )
           return;
    Matrix4d trans = new Matrix4d();

    model .getViewTransform( trans, 0d );
       trans .invert();
       for ( int i = 0; i < mViewers .size(); i++ )
           mViewers .get( i ) .setViewTransformation( trans, Viewer .MONOCULAR );

       model .getStereoViewTransform( trans, Viewer .LEFT_EYE );
       trans .invert();
       for ( int i = 0; i < mViewers .size(); i++ )
           mViewers .get( i ) .setViewTransformation( trans, Viewer .LEFT_EYE );

       model .getStereoViewTransform( trans, Viewer .RIGHT_EYE );
       trans .invert();
       for ( int i = 0; i < mViewers .size(); i++ )
           mViewers .get( i ) .setViewTransformation( trans, Viewer .RIGHT_EYE );
}

/**
 * Creates a new instance of Camera
 * 
 * @param eyePoint
 *            the eyePoint in world coordinates. Origin of the rays.
 * @param lookAtPoint
 *            the point we look at world coordinates.
 * @param upVector
 *            the vector giving the up direction.
 * @param dist
 *            distance between eyepoint and view plane.
 * @param size
 *            height of the view plane.
 */
public Camera(Point3d eyePoint, Point3d lookAtPoint, Vector3d upVector,
        double dist, double size) {
    super();
    eye = eyePoint;
    lookAt = lookAtPoint;
    up = upVector;
    Point3d nouveau = ((Point3d) eye.clone());
    nouveau.sub(lookAt);
    n = new Vector3d(-nouveau.x, -nouveau.y, -nouveau.z);
    n.normalize();
    u = new Vector3d();
    u.cross(up, n);
    u.normalize();
    v = new Vector3d();
    v.cross(n, u);
    matrix = new Matrix4d(u.x, u.y, u.z, -1
            * u.dot(new Vector3d(eye.x, eye.y, eye.z)), v.x, v.y, v.z, -1
            * v.dot(new Vector3d(eye.x, eye.y, eye.z)), n.x, n.y, n.z, -1
            * n.dot(new Vector3d(eye.x, eye.y, eye.z)), 0, 0, 0, 1);
    distanceEyeVpn = dist;
    heightVpn = size;
    eyeFixed = false;
    lookAtFixed = false;
    ;
}

/**
 * Update the parameters of the camera when points have been changed.
 */
public void updateParameters() {
    // update paramaters
    Point3d nouveau = ((Point3d) eye.clone());
    nouveau.sub(lookAt);
    n = new Vector3d(-nouveau.x, -nouveau.y, -nouveau.z);
    n.normalize();
    u = new Vector3d();
    u.cross(up, n);
    u.normalize();
    v = new Vector3d();
    v.cross(n, u);
    matrix = new Matrix4d(u.x, u.y, u.z, -1
            * u.dot(new Vector3d(eye.x, eye.y, eye.z)), v.x, v.y, v.z, -1
            * v.dot(new Vector3d(eye.x, eye.y, eye.z)), n.x, n.y, n.z, -1
            * n.dot(new Vector3d(eye.x, eye.y, eye.z)), 0, 0, 0, 1);
}

/** superimpose and get rmsd
 *
 * @param pro1
 * @param pro2
 * @param strLen Number of atoms from pro1 and pro2 to use
 * @param storeTransform Store rotation and shift matrices locally
 * @return RMSD
 * @throws StructureException
 */
public double calc_rmsd(Atom[] pro1, Atom[] pro2, int strLen, 
        boolean storeTransform) throws StructureException {

    Atom[] cod1 = getAtoms(pro1,  strLen,false);
    Atom[] cod2 = getAtoms(pro2,  strLen,true);

    Matrix4d trans = SuperPositions.superpose(Calc.atomsToPoints(cod1), 
            Calc.atomsToPoints(cod2));

    Matrix matrix = Matrices.getRotationJAMA(trans);
    Atom shift = Calc.getTranslationVector(trans);

    if ( storeTransform) {
        r = matrix;
        t = shift;
    }
    for (Atom a : cod2)
        Calc.transform(a.getGroup(), trans);

    return Calc.rmsd(cod1, cod2);

}

/**
 * Get a list of N 4*4 matrices from a single list of doubles of length 16*N.
 * @param ncsOperMatrixList the input list of doubles
 * @return the list of 4*4 matrics 
 */
public static Matrix4d[] getNcsAsMatrix4d(double[][] ncsOperMatrixList) {
    if(ncsOperMatrixList==null){
        return null;
    }
    int numMats = ncsOperMatrixList.length;
    if(numMats==0){
        return null;
    }
    if(numMats==1 && ncsOperMatrixList[0].length==0){
        return null;
    }
    Matrix4d[] outList = new Matrix4d[numMats];
    for(int i=0; i<numMats; i++){
        outList[i] = new Matrix4d(ncsOperMatrixList[i]);
    }
    return outList;
}

/**
 * Sets the value of this matrix to that of the Matrix4d provided.
 *
 * @param m1
 *            the source matrix
 */
public final void set(Matrix4d m1) {
    // This implementation is in 'no automatic size grow' policy.
    // When size mismatch, exception will be thrown from the below.
    elementData[0] = (float) m1.m00;
    elementData[1] = (float) m1.m01;
    elementData[2] = (float) m1.m02;
    elementData[3] = (float) m1.m03;
    elementData[nCol] = (float) m1.m10;
    elementData[nCol + 1] = (float) m1.m11;
    elementData[nCol + 2] = (float) m1.m12;
    elementData[nCol + 3] = (float) m1.m13;
    elementData[2 * nCol] = (float) m1.m20;
    elementData[2 * nCol + 1] = (float) m1.m21;
    elementData[2 * nCol + 2] = (float) m1.m22;
    elementData[2 * nCol + 3] = (float) m1.m23;
    elementData[3 * nCol] = (float) m1.m30;
    elementData[3 * nCol + 1] = (float) m1.m31;
    elementData[3 * nCol + 2] = (float) m1.m32;
    elementData[3 * nCol + 3] = (float) m1.m33;
}

/**
 * Return the rmsd of the CAs between the input pro and this protein, at
 * given positions. quite similar to transPdb but while that one transforms
 * the whole ca2, this one only works on the res1 and res2 positions.
 *
 * Modifies the coordinates in the second set of Atoms (pro).
 *
 * @return rmsd of CAs
 */
private static double calCaRmsd(Atom[] ca1, Atom[] pro, int resn,
        int[] res1, int[] res2) throws StructureException {

    Atom[] cod1 = getAtoms(ca1, res1, resn, false);
    Atom[] cod2 = getAtoms(pro, res2, resn, false);

    if (cod1.length == 0 || cod2.length == 0) {
        logger.info("length of atoms  == 0!");
        return 99;
    }

    Matrix4d transform = SuperPositions.superpose(Calc.atomsToPoints(cod1),
            Calc.atomsToPoints(cod2));

    for (Atom a : cod2)
        Calc.transform(a.getGroup(), transform);

    return Calc.rmsd(cod1, cod2);
}

@Test
public void testVecmathTransformation() {

    Atom atom = getAtom(1.0, 1.0, 1.0);

    //Identity transform
    Matrix4d ident = new Matrix4d();
    ident.setIdentity();
    Calc.transform(atom, ident);

    Point3d expected = new Point3d(1.0, 1.0, 1.0);
    Point3d actual = atom.getCoordsAsPoint3d();

    assertEquals(expected, actual);

    //Sample transform
    Matrix4d sample = getSampleTransform();
    Calc.transform(atom, sample);

    expected = new Point3d(2.0, 7.0, -1.3);
    actual = atom.getCoordsAsPoint3d();

    assertEquals(expected, actual);
}

/**
 * The symmetry axes of each level are recovered after the symmetry analysis
 * iterations have finished, using the stored MultipleAlignment at each
 * symmetry level.
 * @return SymmetryAxes
 */
private SymmetryAxes recoverAxes(CeSymmResult result) {

    SymmetryAxes axes = new SymmetryAxes();

    for (int m = 0; m < levels.size(); m++) {

        MultipleAlignment align = levels.get(m).getMultipleAlignment();
        Matrix4d axis = align.getBlockSet(0).getTransformations().get(1);
        SymmetryType type = levels.get(m).getAxes().getElementaryAxis(0).getSymmType();
        int order = align.size();

        axes.addAxis(axis, order, type);
    }
    return axes;
}

/**
 * Test to check the conversion of BioassemblyInfo to a primitive map.
 */
@Test
public void testMakePrimitiveBioasembly() {
    double[] testData = new double[] {0.0, 0.1, 0.2, 0.3,
            1.0, 1.1, 1.2, 1.3,
            2.0, 2.1, 2.2, 2.3,
            3.0, 3.1, 3.2, 3.3};
    BioAssemblyInfo bioAssemblyInfo = new BioAssemblyInfo();
    List<BiologicalAssemblyTransformation> transforms = new ArrayList<>();
    BiologicalAssemblyTransformation biologicalAssemblyTransformation = new BiologicalAssemblyTransformation();
    biologicalAssemblyTransformation.setChainId("C");
    biologicalAssemblyTransformation.setTransformationMatrix(new Matrix4d(testData));
    transforms.add(biologicalAssemblyTransformation);
    bioAssemblyInfo.setTransforms(transforms);
    // Map the chain to the second index
    Map<String, Integer> chainIdToIndexMap = new HashMap<>();
    chainIdToIndexMap.put("C", 2);
    // Now do the conversion and test they are the same
    Map<double[], int[]> transMap = MmtfUtils.getTransformMap(bioAssemblyInfo, chainIdToIndexMap);
    assertArrayEquals(testData, (double[]) transMap.keySet().toArray()[0], 0.0);
    assertArrayEquals(new int[] {2} , (int[]) transMap.values().toArray()[0]);
}

/**
 * Test the conversion of a matrix to an array of doubles.
 */
@Test 
public void testConvertToDoubleArray() {
    Matrix4d matrix4d = new Matrix4d();
    matrix4d.m00 = 0.0;
    matrix4d.m01 = 0.1;
    matrix4d.m02 = 0.2;
    matrix4d.m03 = 0.3;
    matrix4d.m10 = 1.0;
    matrix4d.m11 = 1.1;
    matrix4d.m12 = 1.2;
    matrix4d.m13 = 1.3;
    matrix4d.m20 = 2.0;
    matrix4d.m21 = 2.1;
    matrix4d.m22 = 2.2;
    matrix4d.m23 = 2.3;
    matrix4d.m30 = 3.0;
    matrix4d.m31 = 3.1;
    matrix4d.m32 = 3.2;
    matrix4d.m33 = 3.3;
    double[] testData = new double[] {0.0, 0.1, 0.2, 0.3,
            1.0, 1.1, 1.2, 1.3,
            2.0, 2.1, 2.2, 2.3,
            3.0, 3.1, 3.2, 3.3};
    assertArrayEquals(testData,MmtfUtils.convertToDoubleArray(matrix4d), 0.0);
}

/**
 * Convert a bioassembly information into a map of transform, chainindices it relates to.
 * @param bioassemblyInfo  the bioassembly info object for this structure
 * @param chainIdToIndexMap the map of chain ids to the index that chain corresponds to.
 * @return the bioassembly information (as primitive types).
 */
public static Map<double[], int[]> getTransformMap(BioAssemblyInfo bioassemblyInfo, Map<String, Integer> chainIdToIndexMap) {
    Map<Matrix4d, List<Integer>> matMap = new HashMap<>();
    List<BiologicalAssemblyTransformation> transforms = bioassemblyInfo.getTransforms();
    for (BiologicalAssemblyTransformation transformation : transforms) {
        Matrix4d transMatrix = transformation.getTransformationMatrix();
        String transChainId = transformation.getChainId();
        if (!chainIdToIndexMap.containsKey(transChainId)){
            continue;
        }
        int chainIndex = chainIdToIndexMap.get(transformation.getChainId());
        if(matMap.containsKey(transMatrix)){
            matMap.get(transMatrix).add(chainIndex);
        }
        else{
            List<Integer> chainIdList = new ArrayList<>();
            chainIdList.add(chainIndex);
            matMap.put(transMatrix, chainIdList);
        }
    }
    Map<double[], int[]> outMap = new HashMap<>();
    for (Entry<Matrix4d, List<Integer>> entry : matMap.entrySet()) {
        outMap.put(convertToDoubleArray(entry.getKey()), CodecUtils.convertToIntArray(entry.getValue()));
    }
    return outMap;
}

public static AffineTransform3D createRotationOx(double theta) {
    Matrix3d matrix3d = new Matrix3d();
    matrix3d.rotX(theta);
    Matrix4d matrix4d = new Matrix4d();
    matrix4d.set(matrix3d);
    return new AffineTransform3D(matrix4d);
}

public static AffineTransform3D createRotationOy(double theta) {
    Matrix3d matrix3d = new Matrix3d();
    matrix3d.rotY(theta);
    Matrix4d matrix4d = new Matrix4d();
    matrix4d.set(matrix3d);
    return new AffineTransform3D(matrix4d);
}

public static AffineTransform3D createRotationOz(double theta) {
    Matrix3d matrix3d = new Matrix3d();
    matrix3d.rotZ(theta);
    Matrix4d matrix4d = new Matrix4d();
    matrix4d.set(matrix3d);
    return new AffineTransform3D(matrix4d);
}