Java 类javax.vecmath.AxisAngle4f 实例源码

@Override
protected void outputEllipsoid(Point3f center, Point3f[] points, short colix) {
  if (debug) {
    debugPrint("outputEllipsoid: colix="
        + String.format("%04x", new Short(colix)));
    debugPrint("  center=" + center);
    debugPrint("  points[0]=" + points[0]);
    debugPrint("  points[1]=" + points[1]);
    debugPrint("  points[2]=" + points[2]);
  }
  if (surfacesOnly) {
    debugPrint("  Not done owing to surfacesOnly");
    return;
  }

  AxisAngle4f a = Quaternion.getQuaternionFrame(center, points[1], points[3])
      .toAxisAngle4f();
  float sx = points[1].distance(center);
  float sy = points[3].distance(center);
  float sz = points[5].distance(center);
  outputEllipsoid1(center, sx, sy, sz, a, colix);
}

/**
 * Local implementation of outputEllipsoid.
 * 
 * @param center
 * @param rx
 * @param ry
 * @param rz
 * @param a
 * @param colix
 */
private void outputEllipsoid1(Point3f center, float rx, float ry, float rz,
                              AxisAngle4f a, short colix) {
  MeshSurface data = MeshData.getSphereData();
  addTexture(colix, null);
  String name;
  if (center instanceof Atom) {
    Atom atom = (Atom) center;
    name = atom.getAtomName().replaceAll("\\s", "") + "_Atom";
  } else if (rx == ry && rx == rz) {
    // Is a sphere
    name = "Sphere" + sphereNum++;
  } else {
    name = "Ellipsoid" + ellipsoidNum++;
  }
  setSphereMatrix(center, rx, ry, rz, a, sphereMatrix);
  addMesh(name, data, sphereMatrix, sphereMatrix, colix, null, null);
}

protected void outputQuaternionFrame(Point3f ptCenter, Point3f ptX,
                                     Point3f ptY, Point3f ptZ, float yScale,
                                     String pre, String post) {

  //Hey, hey -- quaternions to the rescue!
  // Just send three points to Quaternion to define a plane and return
  // the AxisAngle required to rotate to that position. That's all there is to it.

  tempQ1.set(ptX);
  tempQ2.set(ptY);
  AxisAngle4f a = Quaternion.getQuaternionFrame(ptCenter, tempQ1, tempQ2)
      .toAxisAngle4f();
  if (!Float.isNaN(a.x)) {
    output(" rotation");
    output(pre);
    output(a.x + " " + a.y + " " + a.z + " " + a.angle);
    output(post);
  }
  float sx = ptX.distance(ptCenter);
  float sy = ptY.distance(ptCenter) * yScale;
  float sz = ptZ.distance(ptCenter);
  output(" scale");
  output(pre);
  output(sx + " " + sy + " " + sz);
  output(post);
}

protected void setSphereMatrix(Point3f center, float rx, float ry, float rz,
                               AxisAngle4f a, Matrix4f sphereMatrix) {
  if (a != null) {
    Matrix3f mq = new Matrix3f();
    Matrix3f m = new Matrix3f();
    m.m00 = rx;
    m.m11 = ry;
    m.m22 = rz;
    mq.set(a);
    mq.mul(m);
    sphereMatrix.set(mq);
  } else {
    sphereMatrix.setIdentity();
    sphereMatrix.m00 = rx;
    sphereMatrix.m11 = ry;
    sphereMatrix.m22 = rz;
  }
  sphereMatrix.m03 = center.x;
  sphereMatrix.m13 = center.y;
  sphereMatrix.m23 = center.z;
  sphereMatrix.m33 = 1;
}

public static TRSRTransformation getMatrix(FzOrientation fzo) {
    Quaternion fzq = Quaternion.fromOrientation(fzo.getSwapped());
    javax.vecmath.Matrix4f trans = newMat();
    javax.vecmath.Matrix4f rot = newMat();
    javax.vecmath.Matrix4f r90 = newMat();

    r90.setRotation(new AxisAngle4f(0, 1, 0, (float) Math.PI / 2));

    trans.setTranslation(new javax.vecmath.Vector3f(0.5F, 0.5F, 0.5F));
    javax.vecmath.Matrix4f iTrans = new javax.vecmath.Matrix4f(trans);
    iTrans.invert();
    rot.setRotation(fzq.toJavax());
    rot.mul(r90);

    trans.mul(rot);
    trans.mul(iTrans);

    return new TRSRTransformation(trans);
}

/**
 * {@link jp.ac.fit.asura.nao.misc.MatrixUtils#transform(javax.vecmath.Vector3f, javax.vecmath.AxisAngle4f, float, javax.vecmath.Vector3f)}
 * のためのテスト・メソッド。
 */
public void testTransform() throws Exception {
    Vector3f v = new Vector3f();
    RobotFrame fr = new RobotFrame(null);
    fr.getTranslation().set(new Vector3f());
    fr.getAxis().set(new AxisAngle4f());
    FrameState fs = new FrameState(fr);
    fs.setAngle(0);
    MatrixUtils.transform(v, fs);
    assertTrue(new Vector3f().epsilonEquals(v, 0.0001f));

    FrameState fs2 = new FrameState(RobotTest.createRobot().get(
            Frames.HeadYaw));
    fs2.setAngle(1.0f);
    v = new Vector3f();
    MatrixUtils.transform(v, fs2);
    assertTrue(new Vector3f(0, 160, -20).epsilonEquals(v, 0.0001f));
}

public void testRot2omega() throws Exception {
    Vector3f pyr = new Vector3f(MathUtils.toRadians(0), MathUtils
            .toRadians(20), MathUtils.toRadians(20));
    Matrix3f rot = new Matrix3f();
    MatrixUtils.pyr2rot(pyr, rot);
    System.out.println(rot);

    Vector3f omega = new Vector3f();
    MatrixUtils.rot2omega(rot, omega);
    Matrix3f a = new Matrix3f();
    a.set(new AxisAngle4f(omega, omega.length()));
    System.out.println(pyr);
    System.out.println(omega);
    System.out.println(a);

    MatrixUtils.rot2pyr(rot, pyr);
    System.out.println(pyr);
}

private void init() {
    j3dChildCount = 0;
    j3dChildMap = new HashMap();
    j3dLinkMap = new HashMap();
    sensorList = new ArrayList();
    allParentPaths = new ObjectArray();

    hasChanged = new boolean[LAST_TRANSFORM_INDEX + 1];

    implGroup = new TransformGroup();

    matrix = new Matrix4f();
    tempVec = new Vector3f();
    tempAxis = new AxisAngle4f();
    tempMtx1 = new Matrix4f();
    tempMtx2 = new Matrix4f();

    transform = new Transform3D();
}

/**
 * Private, internal, common initialisation.
 */
private void init() {
    allParentPaths = new ObjectArray();

    transform = new TransformGroup();

    BranchGroup bg = new BranchGroup();
    bg.addChild(transform);

    j3dImplNode = bg;

    axis = new AxisAngle4f();
    trans = new Vector3f();
    implTrans = new Transform3D();
    implTrans.setIdentity();
}

/**
 * Construct a default instance of this node. The defaults are set by the
 * VRML specification.
 */
public J3DTransform() {
    super("Transform");

    hasChanged = new boolean[LAST_TRANSFORM_INDEX + 1];

    implTG = new TransformGroup();
    implGroup = implTG;

    vfCenter = new float[] {0, 0, 0};
    vfRotation = new float[] {0, 0, 1, 0};
    vfScale = new float[] {1, 1, 1};
    vfScaleOrientation = new float[] {0, 0, 1, 0};
    vfTranslation = new float[] {0, 0, 0};

    matrix = new Matrix4f();
    tempVec = new Vector3f();
    tempAxis = new AxisAngle4f();
    tempMtx1 = new Matrix4f();
    tempMtx2 = new Matrix4f();

    transform = new Transform3D();
}

/**
 * Private, internal, common initialisation.
 */
private void init() {
    allParentPaths = new ObjectArray();

    transform = new TransformGroup();

    BranchGroup bg = new BranchGroup();
    bg.addChild(transform);

    j3dImplNode = bg;

    axis = new AxisAngle4f();
    trans = new Vector3f();
    implTrans = new Transform3D();
    implTrans.setIdentity();
}

/**
 * Common initialisation functionality.
 */
private void init() {
    j3dImplNode = new Transform3D();
    listenerList = new ArrayList(1);

    v1 = new Vector3d();
    v2 = new Vector3d();
    v3 = new Vector3d();
    T = new Transform3D();
    C = new Transform3D();
    R = new Transform3D();
    S = new Transform3D();
    al = new AxisAngle4f();

    changedTransform = new Transform3D[1];
    changedTransform[0] = j3dImplNode;
}

/**
 * Common initialisation functionality.
 */
private void init() {
    j3dImplNode = new Transform3D();
    listenerList = new ArrayList(1);

    v1 = new Vector3d();
    v2 = new Vector3d();
    v3 = new Vector3d();
    T = new Transform3D();
    C = new Transform3D();
    R = new Transform3D();
    S = new Transform3D();
    al = new AxisAngle4f();

    changedTransform = new Transform3D[1];
    changedTransform[0] = j3dImplNode;
}

/**
 * Common internal initialisation.
 */
private void init() {
    listenerList = new ArrayList(1);
    oglMatrix = new Matrix4f();

    changedTransforms = new Matrix4f[1];
    changedTransforms[0] = oglMatrix;

    v1 = new Vector3f();
    v2 = new Vector3f();
    v3 = new Vector3f();
    T = new Matrix4f();
    C = new Matrix4f();
    R = new Matrix4f();
    S = new Matrix4f();
    al = new AxisAngle4f();
}

/**
 * Construct a new manager for visiblity sensors.
 */
VisibilityManager() {

    picker = new PickRequest();
    picker.pickType = PickRequest.FIND_VISIBLES;
    picker.pickGeometryType = PickRequest.PICK_FRUSTUM;
    picker.pickSortType = PickRequest.SORT_ALL;
    picker.generateVWorldMatrix = true;
    picker.origin = new float[24];

    viewFrustum = new double[6];

    endPoint = new Point3f();
    viewAngle = new AxisAngle4f();
    localTx = new Matrix4f();
    prjMatrix = new Matrix4f();

    activeVisSensors = new HashSet();
    newVisSensors = new HashSet();

    matrixUtils = new MatrixUtils();
    invWorldScale = 1;

    list = new OGLVisibilityListener[LIST_START_SIZE];
    lastListener = 0;
}

/**
   * Construct a default instance of this node. The defaults are set by the
   * VRML specification.
   */
  public BaseGeoTransform() {
      super("GeoTransform");

      hasChanged = new boolean[NUM_FIELDS];

      vfGeoCenter = new double[3];
      vfRotation = new float[] {0, 0, 1, 0};
      vfScale = new float[] {1, 1, 1};
      vfScaleOrientation = new float[] {0, 0, 1, 0};
      vfTranslation = new float[] {0, 0, 0};
      vfGeoSystem = new String[] {"GD","WE"};

      localCenter = new double[3];
locMtx = new Matrix4f();

      tmatrix = new Matrix4f();

      tempVec = new Vector3f();
      tempVec3d = new Vector3d();
      tempAxis = new AxisAngle4f();
      tempMtx1 = new Matrix4f();
      tempMtx2 = new Matrix4f();
origQuat = new Quat4d();
rotQuat = new Quat4d();
  }

/**
 * Construct a default instance of this node. The defaults are set by the
 * VRML specification.
 */
public BaseTransform() {
    super("Transform");

    hasChanged = new boolean[LAST_TRANSFORM_INDEX + 1];

    vfCenter = new float[] {0, 0, 0};
    vfRotation = new float[] {0, 0, 1, 0};
    vfScale = new float[] {1, 1, 1};
    vfScaleOrientation = new float[] {0, 0, 1, 0};
    vfTranslation = new float[] {0, 0, 0};

    tmatrix = new Matrix4f();

    tempVec = new Vector3f();
    //tempAxis = new Vector4f();
    tempAxis = new AxisAngle4f();
    tempMtx1 = new Matrix4f();
    tempMtx2 = new Matrix4f();
}

/**
 * Construct a default instance of this node. The defaults are set by the
 * VRML specification.
 */
public BaseCADPart() {
    super("CADPart");

    hasChanged = new boolean[LAST_CADPART_INDEX + 1];

    vfCenter = new float[] {0, 0, 0};
    vfRotation = new float[] {0, 0, 1, 0};
    vfScale = new float[] {1, 1, 1};
    vfScaleOrientation = new float[] {0, 0, 1, 0};
    vfTranslation = new float[] {0, 0, 0};

    tmatrix = new Matrix4f();

    tempVec = new Vector3f();
    tempAxis = new AxisAngle4f();
    tempMtx1 = new Matrix4f();
    tempMtx2 = new Matrix4f();
}

/**
 * Construct a new generalised joint node object.
 *
 * @param name The VRML name of this node
 */
public BaseCollidableNode(String name) {
    super(name);

    vfTranslation = new float[3];
    vfRotation = new float[] { 0, 0, 1, 0 };

    vfBboxSize = new float[] {-1, -1, -1};
    vfBboxCenter = new float[] {0, 0, 0};
    vfEnabled = true;

    tmatrix = new Matrix4f();
    tmatrix.setIdentity();
    angleTmp = new AxisAngle4f();
    positionTmp = new Vector3f();
}

public MBJoint(String name, BlockPart part)
{
    this.name = name;
    if(part.partRotation != null)
    {
        float x = 0, y = 0, z = 0;
        switch(part.partRotation.axis)
        {
            case X:
                x = 1;
            case Y:
                y = 1;
            case Z:
                z = 1;
        }
        Quat4f rotation = new Quat4f();
        rotation.set(new AxisAngle4f(x, y, z, 0));
        Matrix4f m = new TRSRTransformation(
            TRSRTransformation.toVecmath(part.partRotation.origin),
            rotation,
            null,
            null).getMatrix();
        m.invert();
        invBindPose = new TRSRTransformation(m);
    }
    else
    {
        invBindPose = TRSRTransformation.identity();
    }
}

/**
 *Sets initial rotation for the model and transforms it.
 */
public void initRotation() {
    float rotationX = 0.0f;
    float rotationY = 0.0f;
    float rotationZ = 0.0f;
  switch (part) {
        case LEFT_EAR:
            rotationY = 40.0f;
            break;
        case RIGHT_EAR:
            rotationY = -40.0f;
            break;
    }
    initialRotation = new Vector3f(rotationX, rotationY, rotationZ);

    initRotQuat = new Quat4f(0, 0, 0, 1);
    if (initialRotation.x != 0) {
        Quat4f xRot = new Quat4f();           
        xRot.set(new AxisAngle4f(1f,0f,0f, (float) Math.toRadians(initialRotation.x)));  
        initRotQuat.mul(xRot,initRotQuat);
    }
    if (initialRotation.y != 0) {
        Quat4f yRot = new Quat4f();
        yRot.set(new AxisAngle4f(0f, 1f, 0f, (float) Math.toRadians(initialRotation.y)));
        initRotQuat.mul(yRot,initRotQuat);
    }

    if (initialRotation.z != 0) {
        Quat4f zRot = new Quat4f();
        zRot.set(new AxisAngle4f(0f, 0f, 1f, (float) Math.toRadians(initialRotation.z)));
        initRotQuat.mul(zRot,initRotQuat);
    }/* */
    transform();
}

/**
 * Sets the initial rotation (basic position) of model.
 * 
 * @param initialRotation initial rotation
 */
public void setInitialRotation(Vector3f initialRotation) {
    Quat4f xRot = new Quat4f();
    xRot.set(new AxisAngle4f(1f, 0f, 0f, (float) Math.toRadians(initialRotation.x)));
    Quat4f yRot = new Quat4f();
    yRot.set(new AxisAngle4f(0f, 1f, 0f, (float) Math.toRadians(initialRotation.y)));
    Quat4f zRot = new Quat4f();
    zRot.set(new AxisAngle4f(0f, 0f, 1f, (float) Math.toRadians(initialRotation.z)));
    initRotQuat = new Quat4f(0, 0, 0, 1);
    initRotQuat.mul(xRot,initRotQuat);
    initRotQuat.mul(yRot,initRotQuat);
    initRotQuat.mul(zRot,initRotQuat);
    this.initialRotation = initialRotation;
}

static void quat() {
    final AxisAngle4f angle1 = new AxisAngle4f(0, 1, 0, RotationMath.toRadians(360+90));
    Log.log.info(angle1+":"+RotationMath.toDegrees(angle1.angle));
    final Quat4f quat1 = RotationMath.toQuat(angle1);
    final AxisAngle4f angle2 = RotationMath.toAxis(quat1);
    Log.log.info(angle2+":"+RotationMath.toDegrees(angle2.angle));
    final Quat4f quat2 = new Quat4f(0, 0, 0, 1);
    quat2.mul(quat1);
    final AxisAngle4f angle3 = RotationMath.toAxis(quat2);
    Log.log.info(angle3+":"+RotationMath.toDegrees(angle3.angle));
}

public static @Nonnull Quat4f toQuat(final @Nonnull AxisAngle4f axis) {
    if (axis.angle==0f)
        return newQuat();
    final Quat4f q = new Quat4f();
    q.set(axis);
    return q;
}

@Override
public @Nonnull DiffRotation diff(@Nullable KeyRotation base) {
    if (base==null)
        base = new BaseRotation();
    final AxisAngle4f axis = new AxisAngle4f(this.x, this.y, this.z, this.angle);
    final Builder<Rotate> builder = ImmutableList.builder();
    for (final ImageRotate rotate : this.rotates)
        builder.add(rotate.build());
    return new DiffRotation(base, axis, builder.build());
}

@SuppressWarnings("unchecked")
public static String escape(Object x) {
  if (x instanceof String)
    return escape((String) x);
  if (x instanceof List<?>)
    return escape((ArrayList<ScriptVariable>) x);
  if (x instanceof BitSet) 
    return escape((BitSet) x, true);
  if (x instanceof Matrix3f) 
    return ((Matrix3f) x).toString();
  if (x instanceof Matrix4f) 
    return ((Matrix4f) x).toString();
  if (x instanceof Tuple3f)
    return escape((Tuple3f) x);
  if (x instanceof Point4f) {
    Point4f xyzw = (Point4f) x;
    return "{" + xyzw.x + " " + xyzw.y + " " + xyzw.z + " " + xyzw.w + "}";
  }
  if (x instanceof AxisAngle4f) {
    AxisAngle4f a = (AxisAngle4f) x;
    return "{" + a.x + " " + a.y + " " + a.z + " " + (float) (a.angle * 180d/Math.PI) + "}";
  }    
  if (x instanceof String[])
    return escape((String[]) x, true);
  if (x instanceof int[] 
        || x instanceof int[][]
        || x instanceof float[]
        || x instanceof float[][]
        || x instanceof float[][][]) 
    return toJSON(null, x);
  if (x instanceof Point3f[])
    return escapeArray(x);
  if (x instanceof Map)
    return escape((Map<String, Object>) x);
  return (x == null ? "null" : x.toString());
}

public void set(AxisAngle4f a) {
  AxisAngle4f aa = new AxisAngle4f(a);
  if (aa.angle == 0)
    aa.y = 1;
  Matrix3f m3 = new Matrix3f();
  m3.set(aa);
  set(m3);
}

public AxisAngle4f toAxisAngle4f() {
  fixQ(qTemp);
  double theta = 2 * Math.acos(qTemp.q0);
  double sinTheta2 = Math.sin(theta/2);
  Vector3f v = getNormal();
  if (sinTheta2 < 0) {
    v.scale(-1);
    theta = Math.PI - theta;
  }
  return new AxisAngle4f(v, (float) theta);
}

Map<String, Object> getOrientationInfo() {
  Map<String, Object> info = new Hashtable<String, Object>();
  info.put("moveTo", getMoveToText(1, false));
  info.put("center", "center " + getCenterText());
  info.put("centerPt", fixedRotationCenter);
  AxisAngle4f aa = new AxisAngle4f();
  getAxisAngle(aa);
  info.put("axisAngle", aa);
  info.put("quaternion", new Quaternion(aa).toPoint4f());
  info.put("rotationMatrix", matrixRotate);
  info.put("rotateZYZ", getRotateZyzText(false));
  info.put("rotateXYZ", getRotateXyzText());
  info.put("transXPercent", new Float(getTranslationXPercent()));
  info.put("transYPercent", new Float(getTranslationYPercent()));
  info.put("zoom", new Float(zoomPercent));
  info.put("modelRadius", new Float(modelRadius));
  if (mode == MODE_NAVIGATION) {
    info.put("navigationCenter", "navigate center "
        + Escape.escape(navigationCenter));
    info.put("navigationOffsetXPercent", new Float(
        getNavigationOffsetPercent('X')));
    info.put("navigationOffsetYPercent", new Float(
        getNavigationOffsetPercent('Y')));
    info
        .put("navigationDepthPercent", new Float(getNavigationDepthPercent()));
  }
  return info;
}

@Override
protected void outputEllipsoid(Point3f center, Point3f[] points, short colix) {
  //Hey, hey -- quaternions to the rescue!
  // Just send three points to Quaternion to define a plane and return
  // the AxisAngle required to rotate to that position. That's all there is to it.

  AxisAngle4f a = Quaternion.getQuaternionFrame(center, points[1], points[3]).toAxisAngle4f();
  float sx = points[1].distance(center);
  float sy = points[3].distance(center);
  float sz = points[5].distance(center);
  setSphereMatrix(center, sx, sy, sz, a, sphereMatrix);
  outputEllipsoid(center, sphereMatrix, colix);
}

public void setViewPosition(Vector3f mov, int angle) {
    TransformGroup tg = this.simpleUniverse.getViewingPlatform().
            getViewPlatformTransform();
    Transform3D t3dTrans = new Transform3D();
    tg.getTransform(t3dTrans);
    t3dTrans.setTranslation(mov);
    tg.setTransform(t3dTrans);

    Transform3D t3dRot = new Transform3D();
    tg.getTransform(t3dRot);
    t3dRot.setRotation(
            new AxisAngle4f(1f, 0f, 0f, (float) Math.toRadians(angle)));
    //ViewT3D.setRotation(new AxisAngle4f(0, 0, 1, (float) Math.toRadians(45)));
    tg.setTransform(t3dRot);
}

public TransformGroup rotateNode(Node n) {
    Transform3D transRot = new Transform3D();
    TransformGroup tg = new TransformGroup();
    tg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
    tg.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
    tg.setCapability(TransformGroup.ENABLE_PICK_REPORTING);

    Vector3f cross2 = new Vector3f(1.0f, 0.0f, 0.0f);
    transRot.setRotation(new AxisAngle4f(cross2, (float) Math.toRadians(90)));
    tg.setTransform(transRot);
    return tg;
}

public void setViewPosition(Vector3f mov, int angle) {
    TransformGroup tg = this.simpleUniverse.getViewingPlatform().getViewPlatformTransform();
    Transform3D t3dTrans = new Transform3D();
    tg.getTransform(t3dTrans);
    t3dTrans.setTranslation(mov);
    tg.setTransform(t3dTrans);

    Transform3D t3dRot = new Transform3D();
    tg.getTransform(t3dRot);
    t3dRot.setRotation(new AxisAngle4f(1f, 0f, 0f, (float) Math.toRadians(angle)));
    //ViewT3D.setRotation(new AxisAngle4f(0, 0, 1, (float) Math.toRadians(45)));
    tg.setTransform(t3dRot);
}

public void setViewPosition(Vector3f mov, int angle) {
    TransformGroup tg = this.simpleU.getViewingPlatform().getViewPlatformTransform();

    Transform3D t3dTrans = new Transform3D();
    tg.getTransform(t3dTrans);
    t3dTrans.setTranslation(mov);
    tg.setTransform(t3dTrans);

    Transform3D t3dRot = new Transform3D();
    tg.getTransform(t3dRot);
    //t3dRot.setTranslation(mov);
    t3dRot.setRotation(new AxisAngle4f(1f, 0f, 0f, (float) Math.toRadians(angle)));
    //ViewT3D.setRotation(new AxisAngle4f(0, 0, 1, (float) Math.toRadians(45)));
    tg.setTransform(t3dRot);
}

private TransformGroup moveBack(Vector3f back, double scale) {
    Transform3D transform3D = new Transform3D();
    //transform3D.setTranslation(new Vector3f(0.0f, 0.0f, 0.0f));
    //transform3D.setTranslation(new Vector3f(0f, -20f, 0f));
    transform3D.setRotation(new AxisAngle4f(1.0f, 0.0f, 0.0f, (float) Math.toRadians(270)));
    //new Vector3f(-0.3f, -0.8f, -3.0f)
    transform3D.setTranslation(back);
    transform3D.setScale(scale);
    return new TransformGroup(transform3D);
}

public AnimationCaptureController( CameraController cameraController, File directory )
{       
    this .iteration = NUM_ITERATIONS;
    double rotationRadians = 2 * Math.PI / NUM_ITERATIONS;

    this.cameraController = cameraController;
    Vector3f viewRotAxis = new Vector3f( 0f, 1.618f, 1f );
    this .cameraController .mapViewToWorld( viewRotAxis );
    this .rotation = new Quat4d();
    this .rotation .set( new AxisAngle4f( viewRotAxis, (float) rotationRadians ) );

    this .parentDir = directory .isDirectory()? directory : directory .getParentFile();
}

public AnimationCaptureController( CameraController cameraController, File directory )
{       
    this .iteration = NUM_ITERATIONS;
    double rotationRadians = 2 * Math.PI / NUM_ITERATIONS;

    this.cameraController = cameraController;
    Vector3f viewRotAxis = new Vector3f( 0f, 1.618f, 1f );
    this .cameraController .mapViewToWorld( viewRotAxis );
    this .rotation = new Quat4d();
    this .rotation .set( new AxisAngle4f( viewRotAxis, (float) rotationRadians ) );

    this .parentDir = directory .isDirectory()? directory : directory .getParentFile();
}

@Override
public void renderTransparent() {

    Vector3f cameraPosition = worldRenderer.getActiveCamera().getPosition();
    ShaderProgram shader = ShaderManager.getInstance().getShaderProgram("gelatinousCube");
    shader.enable();

    for (EntityRef entity : gelatinous) {
        MeshComponent meshComp = entity.getComponent(MeshComponent.class);
        LocationComponent location = entity.getComponent(LocationComponent.class);
        if (location == null) continue;

        Quat4f worldRot = location.getWorldRotation();
        Vector3f worldPos = location.getWorldPosition();
        float worldScale = location.getWorldScale();
        AABB aabb = gelatinousCubeMesh.getAABB().transform(worldRot, worldPos, worldScale);
        if (worldRenderer.isAABBVisible(aabb)) {
            glPushMatrix();

            glTranslated(worldPos.x - cameraPosition.x, worldPos.y - cameraPosition.y, worldPos.z - cameraPosition.z);
            AxisAngle4f rot = new AxisAngle4f();
            rot.set(worldRot);
            glRotatef(TeraMath.RAD_TO_DEG * rot.angle, rot.x, rot.y, rot.z);
            glScalef(worldScale, worldScale, worldScale);

            shader.setFloat4("colorOffset", meshComp.color.x, meshComp.color.y, meshComp.color.z, meshComp.color.w);
            shader.setFloat("light", worldRenderer.getRenderingLightValueAt(worldPos));

            gelatinousCubeMesh.render();

            glPopMatrix();
        }
    }
}

private void ghost(float delta, EntityRef entity, LocationComponent location, CharacterMovementComponent movementComp) {
    Vector3f desiredVelocity = new Vector3f(movementComp.getDrive());
    float maxSpeed = movementComp.maxGhostSpeed;
    if (movementComp.isRunning) {
        maxSpeed *= movementComp.runFactor;
    }

    desiredVelocity.scale(maxSpeed);

    // Modify velocity towards desired, up to the maximum rate determined by friction
    Vector3f velocityDiff = new Vector3f(desiredVelocity);
    velocityDiff.sub(movementComp.getVelocity());

    velocityDiff.scale(Math.min(GHOST_INERTIA * delta, 1.0f));

    movementComp.getVelocity().add(velocityDiff);

    // No collision, so just do the move
    Vector3f worldPos = location.getWorldPosition();
    Vector3f deltaPos = new Vector3f(movementComp.getVelocity());
    deltaPos.scale(delta);
    worldPos.add(deltaPos);
    location.setWorldPosition(worldPos);
    if (deltaPos.length() > 0) {
        entity.send(new MovedEvent(deltaPos, worldPos));
    }

    movementComp.collider.setWorldTransform(new Transform(new Matrix4f(new Quat4f(0, 0, 0, 1), worldPos, 1.0f)));

    if (movementComp.faceMovementDirection && movementComp.getVelocity().lengthSquared() > 0.01f) {
        float yaw = (float) Math.atan2(movementComp.getVelocity().x, movementComp.getVelocity().z);
        AxisAngle4f axisAngle = new AxisAngle4f(0, 1, 0, yaw);
        location.getLocalRotation().set(axisAngle);
    }
}

private static Multimap<Orientation, XYZRotation> calculateXyzRotations(Map<Matrix3f, Orientation> fromMatrix) {
    final Multimap<Orientation, XYZRotation> toXYZRotation = HashMultimap.create();
    for (Rotation x : Rotation.values())
        for (Rotation y : Rotation.values())
            for (Rotation z : Rotation.values()) {
                final XYZRotation rotation = new XYZRotation(x, y, z);
                Quat4f q = new Quat4f(0, 0, 0, 1);

                Quat4f tmp = new Quat4f();
                tmp.set(new AxisAngle4f(0, 0, 1, z.angle));
                q.mul(tmp);

                tmp.set(new AxisAngle4f(0, 1, 0, y.angle));
                q.mul(tmp);

                tmp.set(new AxisAngle4f(1, 0, 0, x.angle));
                q.mul(tmp);

                Matrix3f m = new Matrix3f();
                m.set(q);
                roundMatrixElements(m);
                final Orientation orientation = fromMatrix.get(m);
                Preconditions.checkNotNull(orientation, rotation);
                toXYZRotation.put(orientation, rotation);
            }

    return toXYZRotation;
}