Java 类com.badlogic.gdx.math.Quaternion 实例源码

public Basic(Matrix4 transform, double speeed, int hp, int health, int range, float coolDown, EnumSet<Types> types, EnumSet<Effects> effects, ModelInstance instance, btCollisionWorld world, IntMap<Entity> entities, List<Vector3> path, Map<String, Sound> sounds) {
    super(transform, speeed, hp, health, range, coolDown, types, effects, instance, new btCompoundShape(), world, entities, ATTACK_ANIMATION, ATTACK_OFFSET, path, sounds);
    ((btCompoundShape)shape).addChildShape(new Matrix4(new Vector3(0, 30, 0), new Quaternion().setEulerAngles(0, 0, 0), new Vector3(1, 1, 1)), new btBoxShape(new Vector3(75, 30, 90)));
    //System.out.println(getModelInstance().getAnimation("Spider_Armature|walk_ani_vor").id);
    listener = new AnimationController.AnimationListener() {
        @Override
        public void onEnd(AnimationController.AnimationDesc animationDesc) {

        }

        @Override
        public void onLoop(AnimationController.AnimationDesc animationDesc) {

        }
    };
    //animation.setAnimation("Spider_Armature|walk_ani_vor");
    //animation.animate("Spider_Armature|walk_ani_vor", -1);
    //animation.action("Spider_Armature|walk_ani_vor", 0, 1000, -1, 1, listener, 0);
    //animation.animate("Spider_Armature|Attack", 0, 1000, 1, 1, listener, 0);
    //animation.queue("Spider_Armature|walk_ani_vor", 0, 1000, -1, 1, listener, 0);
}

private void processBodyComponent(RigidBodyComponent bodyComponent, NodeComponent nodeComponent) {
    if (bodyComponent.state == RigidBodyComponent.State.READY) {
        if (!bodyComponent.isAdded()) {
            Matrix4 initialTransform = new Matrix4();

            Vector3 translation = nodeComponent.node.translation;
            Vector3 scale = new Vector3(1, 1, 1);
            Quaternion rotation = nodeComponent.node.rotation;

            initialTransform.set(translation, rotation, scale);
            bodyComponent.addToWorld(dynamicsWorld, initialTransform);
        } else {
            nodeComponent.setTranslation(bodyComponent.getTranslation());
            nodeComponent.setRotation(bodyComponent.getRotation());
            nodeComponent.applyTransforms();
        }
    }
}

private void processVehicleComponent(VehicleComponent vehicleComponent, NodeComponent nodeComponent) {
    if (vehicleComponent.state == RigidBodyComponent.State.READY) {
        if (!vehicleComponent.isAdded()) {
            Matrix4 initialTransform = new Matrix4();

            Vector3 trn = nodeComponent.getTranslation();
            Vector3 scl = new Vector3(1, 1, 1);
            Quaternion rtn = nodeComponent.getRotationQuaternion();

            initialTransform.set(trn, rtn, scl);

            vehicleComponent.addToWorld(dynamicsWorld, initialTransform);
        } else {
            nodeComponent.setTranslation(vehicleComponent.getTranslation());
            nodeComponent.setRotation(vehicleComponent.getRotation());
            nodeComponent.applyTransforms();
        }
    }
}

public NodeComponent() {
    node = new Node();

    tempVec = new Vector3();
    tempVec2 = new Vector3();
    tempQuat = new Quaternion();

    translation = new Vector3();
    rotation = new Vector3();
    scale = new Vector3(1, 1, 1);

    localTranslation = new Vector3();
    localRotation = new Vector3();
    localScale = new Vector3(1, 1, 1);

    translationDelta = new Vector3();
    rotationDelta = new Vector3();
    scaleDelta = new Vector3();

    localRotationQuaternion = new Quaternion();
    rotationQuaternion = new Quaternion();
}

public static int maxAxis4 (Quaternion v) {
    int maxIndex = -1;
    float maxVal = -1e30f;
    if (v.x > maxVal) {
        maxIndex = 0;
        maxVal = v.x;
    }
    if (v.y > maxVal) {
        maxIndex = 1;
        maxVal = v.y;
    }
    if (v.z > maxVal) {
        maxIndex = 2;
        maxVal = v.z;
    }
    if (v.w > maxVal) {
        maxIndex = 3;
        maxVal = v.w;
    }

    return maxIndex;
}

public static Quaternion shortestArcQuat (Vector3 v0, Vector3 v1, Quaternion out) {
    Stack stack = Stack.enter();
    Vector3 c = stack.allocVector3();
    c.set(v0).crs(v1);
    float d = v0.dot(v1);

    if (d < -1.0 + BulletGlobals.FLT_EPSILON) {
        // just pick any vector
        out.set(0.0f, 1.0f, 0.0f, 0.0f);
        stack.leave();
        return out;
    }

    float s = (float)Math.sqrt((1.0f + d) * 2.0f);
    float rs = 1.0f / s;

    out.set(c.x * rs, c.y * rs, c.z * rs, s * 0.5f);
    stack.leave();
    return out;
}

public static void setRotation (Matrix3 dest, Quaternion q) {
    float d = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
    assert (d != 0f);
    float s = 2f / d;
    float xs = q.x * s, ys = q.y * s, zs = q.z * s;
    float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
    float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
    float yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
    dest.val[Matrix3.M00] = 1f - (yy + zz);
    dest.val[Matrix3.M01] = xy - wz;
    dest.val[Matrix3.M02] = xz + wy;
    dest.val[Matrix3.M10] = xy + wz;
    dest.val[Matrix3.M11] = 1f - (xx + zz);
    dest.val[Matrix3.M12] = yz - wx;
    dest.val[Matrix3.M20] = xz - wy;
    dest.val[Matrix3.M21] = yz + wx;
    dest.val[Matrix3.M22] = 1f - (xx + yy);
}

public static void getRotation (Matrix3 mat, Quaternion dest) {
    /*
     * ArrayPool<float[]> floatArrays = ArrayPool.get(float.class);
     * 
     * float trace = mat.val[Matrix3.M00] + mat.val[Matrix3.M11] + mat.val[Matrix3.M22]; float[] temp = floatArrays.getFixed(4);
     * 
     * if (trace > 0f) { float s = (float) Math.sqrt(trace + 1f); temp[3] = (s * 0.5f); s = 0.5f / s;
     * 
     * temp[0] = ((mat.val[Matrix3.M21] - mat.val[Matrix3.M12]) * s); temp[1] = ((mat.val[Matrix3.M02] - mat.val[Matrix3.M20]) *
     * s); temp[2] = ((mat.val[Matrix3.M10] - mat.val[Matrix3.M01]) * s); } else { int i = mat.val[Matrix3.M00] <
     * mat.val[Matrix3.M11] ? (mat.val[Matrix3.M11] < mat.val[Matrix3.M22] ? 2 : 1) : (mat.val[Matrix3.M00] <
     * mat.val[Matrix3.M22] ? 2 : 0); int j = (i + 1) % 3; int k = (i + 2) % 3;
     * 
     * float s = (float) Math.sqrt(mat.getElement(i, i) - mat.getElement(j, j) - mat.getElement(k, k) + 1f); temp[i] = s * 0.5f;
     * s = 0.5f / s;
     * 
     * temp[3] = (mat.getElement(k, j) - mat.getElement(j, k)) * s; temp[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
     * temp[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s; } dest.set(temp[0], temp[1], temp[2], temp[3]);
     * 
     * floatArrays.release(temp);
     */
    // FIXME check this is correct
    dest.setFromMatrix(true, mat);
}

public PlaneIntersectionType plane_classify (Quaternion plane) {
    Stack stack = Stack.enter();
    Vector3 tmp = stack.allocVector3();

    float[] _fmin = new float[1], _fmax = new float[1];
    tmp.set(plane.x, plane.y, plane.z);
    projection_interval(tmp, _fmin, _fmax);

    if (plane.w > _fmax[0] + BOX_PLANE_EPSILON) {
        stack.leave();
        return PlaneIntersectionType.BACK_PLANE; // 0
    }

    if (plane.w + BOX_PLANE_EPSILON >= _fmin[0]) {
        stack.leave();
        return PlaneIntersectionType.COLLIDE_PLANE; // 1
    }

    stack.leave();
    return PlaneIntersectionType.FRONT_PLANE; // 2
}

/** Line plane collision.
 * 
 * @return -0 if the ray never intersects, -1 if the ray collides in front, -2 if the ray collides in back */
public static int line_plane_collision (Quaternion plane, Vector3 vDir, Vector3 vPoint, Vector3 pout, float[] tparam, float tmin,
    float tmax) {
    float _dotdir = VectorUtil.dot3(vDir, plane);

    if (Math.abs(_dotdir) < PLANEDIREPSILON) {
        tparam[0] = tmax;
        return 0;
    }

    float _dis = ClipPolygon.distance_point_plane(plane, vPoint);
    int returnvalue = _dis < 0.0f ? 2 : 1;
    tparam[0] = -_dis / _dotdir;

    if (tparam[0] < tmin) {
        returnvalue = 0;
        tparam[0] = tmin;
    } else if (tparam[0] > tmax) {
        returnvalue = 0;
        tparam[0] = tmax;
    }
    pout.x = vPoint.x + tparam[0] * vDir.x;
    pout.y = vPoint.y + tparam[0] * vDir.y;
    pout.z = vPoint.z + tparam[0] * vDir.z;
    return returnvalue;
}

public void moveShipLeft (float delta, float scale) {
        if (ship.isExploding) return;

        float q0 = (float) Invaders.mInvaderInterface.getQ0();
        float q1 = (float) Invaders.mInvaderInterface.getQ1();
        float q2 = (float) Invaders.mInvaderInterface.getQ2();
        float q3 = (float) Invaders.mInvaderInterface.getQ3();

        ship.transform.trn(-delta * Ship.SHIP_VELOCITY * scale, 0, 0);
        ship.transform.getTranslation(tmpV1);
        if (tmpV1.x < PLAYFIELD_MIN_X) ship.transform.trn(PLAYFIELD_MIN_X - tmpV1.x, 0, 0);

        Vector3 oldTranslation = ship.transform.getTranslation(tmpV1);
        Quaternion rotateQ = new Quaternion(q0,-1*q1,q3,-1*q2); //Used if you want all 3-axis rotation
//      Quaternion rotateQ = new Quaternion(1,0,0,0); //Used if you only want the roll

        ship.transform.setToRotation(0, 0, 0, 0);
        ship.transform.set(oldTranslation, rotateQ);

//      ROLL-ONLY CODE - BETTER FOR GAME MODE
//      float roll_x = 57.29578f*((float)(Math.atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))));//Used if you want only roll
//      ship.transform.rotate(0, 0, 1, roll_x);//Used if you want only roll
//      ship.transform.rotate(rotateQ);
    }

public void moveShipRight (float delta, float scale) {
        if (ship.isExploding) return;

        float q0 = (float) Invaders.mInvaderInterface.getQ0();
        float q1 = (float) Invaders.mInvaderInterface.getQ1();
        float q2 = (float) Invaders.mInvaderInterface.getQ2();
        float q3 = (float) Invaders.mInvaderInterface.getQ3();

        ship.transform.trn(+delta * Ship.SHIP_VELOCITY * scale, 0, 0);
        if (tmpV1.x > PLAYFIELD_MAX_X) ship.transform.trn(PLAYFIELD_MAX_X - tmpV1.x, 0, 0);

        Vector3 oldTranslation = ship.transform.getTranslation(tmpV1);

        Vector3 flip = new Vector3(0,0,0);

        Quaternion rotateQ = new Quaternion(q0,-1*q1,q3,-1*q2); //For 3-axis
//      Quaternion rotateQ = new Quaternion(1, 0, 0, 0); //For ROLL-ONLY
//      ship.transform.set(oldTranslation.mulAdd(flip, -1), rotateQ);//ROLL-ONLY
//      float roll_x = 57.29578f*((float)(Math.atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))));//ROLL ONLY
//      ship.transform.rotate(0,0,1,roll_x);//ROLL ONLY


        ship.transform.setToRotation(0, 0, 0, 0);
        ship.transform.set(oldTranslation.mulAdd(flip, -1), rotateQ);
    }

public static void initWorld(BulletWorld world) {
    //TreeShape
    Model model = Assets.get(Models.MODEL_TREE_PROTOTYPE, Model.class);
    model.nodes.first().translation.set(0, -1.15f, 0);
    btCompoundShape treeShape = new btCompoundShape();
    treeShape.addChildShape(new Matrix4(new Vector3(0, 0, 0), new Quaternion(), new Vector3(1, 1, 1)), new btBoxShape(new Vector3(.2f, .9f, .2f)));
    treeShape.addChildShape(new Matrix4(new Vector3(0, 1, 0), new Quaternion(), new Vector3(1, 1, 1)), new btSphereShape(1));
    //LogShape
    model = Assets.get(Models.MODEL_LOG_PROTOTYPE, Model.class);
    model.nodes.first().translation.set(0, -1.15f, 0);

    world.addConstructor("log", new BulletConstructor(Assets.get(Models.MODEL_LOG_PROTOTYPE, Model.class), 75, new btBoxShape(new Vector3(.2f, .9f, .2f))));
    world.addConstructor("stump", new BulletConstructor(Assets.get(Models.MODEL_STUMP_PROTOTYPE, Model.class), 0, new btCylinderShape(new Vector3(.2f, .22f, .2f))));
    world.addConstructor("staticTree", new BulletConstructor(Assets.get(Models.MODEL_TREE_PROTOTYPE, Model.class), 0, treeShape));
    world.addConstructor("dynamicTree", new BulletConstructor(Assets.get(Models.MODEL_TREE_PROTOTYPE, Model.class), 100, treeShape));
}

private void drawArmatureNodes(Node currentNode, Vector3 modelPos,
                               Quaternion modelRot,
                               Vector3 parentNodePos, Vector3 currentNodePos) {
    currentNode.globalTransform.getTranslation(currentNodePos);
    modelRot.transform(currentNodePos);
    currentNodePos.add(modelPos);
    drawVertex(currentNodePos, 0.02f, Color.GREEN);
    shapeRenderer.setColor(Color.YELLOW);
    if (currentNode.hasParent()) {
        shapeRenderer.line(parentNodePos, currentNodePos);
    }
    if (currentNode.hasChildren()) {
        float x = currentNodePos.x;
        float y = currentNodePos.y;
        float z = currentNodePos.z;
        for (Node child : currentNode.getChildren()) {
            drawArmatureNodes(child, modelPos, modelRot, currentNodePos, parentNodePos);
            currentNodePos.set(x, y, z);
        }
    }
}

@Override
public void render() {
    float deltaTime = Gdx.graphics.getDeltaTime();

    if (Gdx.input.isKeyPressed(Input.Keys.W)) {
        VirtualReality.body.position.add(new Vector3(0, 0, -2).mul(VirtualReality.body.orientation).scl(deltaTime));
    }
    if (Gdx.input.isKeyPressed(Input.Keys.S)) {
        VirtualReality.body.position.add(new Vector3(0, 0, 2).mul(VirtualReality.body.orientation).scl(deltaTime));
    }
    if (Gdx.input.isKeyPressed(Input.Keys.A)) {
        VirtualReality.body.orientation.mulLeft(new Quaternion(Vector3.Y, 90f * deltaTime));
    }
    if (Gdx.input.isKeyPressed(Input.Keys.D)) {
        VirtualReality.body.orientation.mulLeft(new Quaternion(Vector3.Y, -90f * deltaTime));
    }

    VirtualReality.update(Gdx.graphics.getDeltaTime());
    VirtualReality.renderer.render();
}

private void init(String tex0, float w, float h) {
    setTexture0(tex0);

    // Init comparator
    comp = new DistToCameraComparator<IRenderable>();
    // Init vertices
    float[] vertices = new float[20];
    fillVertices(vertices, w, h);

    // We wont need indices if we use GL_TRIANGLE_FAN to draw our quad
    // TRIANGLE_FAN will draw the verts in this order: 0, 1, 2; 0, 2, 3
    mesh = new Mesh(VertexDataType.VertexArray, true, 4, 6, new VertexAttribute(Usage.Position, 2, ShaderProgram.POSITION_ATTRIBUTE), new VertexAttribute(Usage.ColorPacked, 4, ShaderProgram.COLOR_ATTRIBUTE), new VertexAttribute(Usage.TextureCoordinates, 2, ShaderProgram.TEXCOORD_ATTRIBUTE + "0"));

    mesh.setVertices(vertices, 0, vertices.length);
    mesh.getIndicesBuffer().position(0);
    mesh.getIndicesBuffer().limit(6);

    short[] indices = new short[] { 0, 1, 2, 0, 2, 3 };
    mesh.setIndices(indices);

    quaternion = new Quaternion();
    aux = new Vector3();

}

private void init(float w, float h) {
    // Init comparator
    comp = new DistToCameraComparator<IRenderable>();
    // Init vertices
    float[] vertices = new float[20];
    fillVertices(vertices, w, h);

    // We wont need indices if we use GL_TRIANGLE_FAN to draw our quad
    // TRIANGLE_FAN will draw the verts in this order: 0, 1, 2; 0, 2, 3
    mesh = new Mesh(VertexDataType.VertexArray, true, 4, 6, new VertexAttribute(Usage.Position, 2, ShaderProgram.POSITION_ATTRIBUTE), new VertexAttribute(Usage.ColorPacked, 4, ShaderProgram.COLOR_ATTRIBUTE), new VertexAttribute(Usage.TextureCoordinates, 2, ShaderProgram.TEXCOORD_ATTRIBUTE + "0"));

    mesh.setVertices(vertices, 0, vertices.length);
    mesh.getIndicesBuffer().position(0);
    mesh.getIndicesBuffer().limit(6);

    short[] indices = new short[] { 0, 1, 2, 0, 2, 3 };
    mesh.setIndices(indices);

    quaternion = new Quaternion();
    aux = new Vector3();

}

public Entity(Matrix4 transform, int hp, int health, EnumSet<Types> types, EnumSet<Effects> effects, ModelInstance instance, btCollisionShape shape, btCollisionWorld world, IntMap<Entity> entities, Map<String, Sound> sounds){
    this.instance = instance;
    this.transform = transform;
    this.hp = hp;
    this.types = types;
    this.health = health;
    this.effects = effects;
    this.sounds = sounds;
    animation = new AnimationController(instance);
    this.instance.transform.set(transform);
    this.shape = shape;
    body = new btCollisionObject();
    body.setCollisionShape(shape);
    body.setWorldTransform(this.instance.transform);
    this.world = world;
    tempVector = new Vector3();
    tempVector2 = new Vector3();
    this.entities = entities;
    tempQuaternion = new Quaternion();
    quaternion = new Quaternion();
    if(this instanceof Enemy || this instanceof Projectile)
        body.setCollisionFlags(body.getCollisionFlags());
    int index = getNextIndex();
    entities.put(index, this);
    body.setUserValue(index);
    world.addCollisionObject(body);
    boundingBox = instance.calculateBoundingBox(new BoundingBox());
    //for(Node node: instance.nodes)
        //System.out.println();
}

@Override
public void write(Kryo kryo, Output output, Quaternion quaternion) {
    output.writeFloat(quaternion.x);
    output.writeFloat(quaternion.y);
    output.writeFloat(quaternion.z);
    output.writeFloat(quaternion.w);
}

@Override
public Quaternion read(Kryo kryo, Input input, Class<Quaternion> type) {
    float x = input.readFloat();
    float y = input.readFloat();
    float z = input.readFloat();
    float w = input.readFloat();
    return new Quaternion(x, y, z, w);
}

public void lookAt (Vector3 point) {
    Vector3 from = transform.getTranslation(new Vector3()).cpy();
    Vector3 to = point.cpy();
    Vector3 direction = to.sub(from).nor();
    direction.set(-direction.x, -direction.y, -direction.z);

    Quaternion quaternion = new Quaternion();
    Matrix4 instanceRotation = transform.cpy().mul(transform);

    instanceRotation.setToLookAt(direction, new Vector3(0,-1,0));
    instanceRotation.rotate(0, 0, 1, 180);
    instanceRotation.getRotation(quaternion);

    transform.set(from, quaternion);
}

@Override
protected void reset() {
    node.translation.set(new Vector3());
    node.rotation.set(new Quaternion());
    node.scale.set(new Vector3());

    translationDelta.set(Vector3.Zero);
    rotationDelta.set(Vector3.Zero);
    scaleDelta.set(Vector3.Zero);
}

public void setRotation(Quaternion rotation) {
    rotationDelta.set(
            rotation.x - rotation.getPitch(),
            rotation.y - rotation.getYaw(),
            rotation.z - rotation.getRoll());

    node.rotation.set(rotation);
}

/**
 * Rotates the node.
 *
 * @param rotation the rotation quaternion added to the node's rotation.
 * @param apply    if true, transforms will be calculated immediately. It's not recommended, instead use
 *                 {@link #applyTransforms() applyTransforms()} after you've completed all transforms on the node.
 */
public void rotate(Quaternion rotation, boolean apply) {
    node.rotation.add(rotation);

    if (apply) {
        applyTransforms();
    }
}

public void build() {
    translation = new Vector3();
    rotation = new Quaternion();

    vehicleComponent.addWheel(attachmentPoint, direction, axis, radius,
            suspensionRestLength, wheelFriction, frontWheel);
}

public RigidBodyComponent() {
    state = State.NOT_INITIALIZED;

    translation = new Vector3();
    scale = new Vector3();
    rotation = new Quaternion();
    initialTransform = new Matrix4();
}

@Override
public void getPlane(Vector3 planeNormal, Vector3 planeSupport, int i) {
    // this plane might not be aligned...
    Stack stack = Stack.enter();
    Quaternion plane = stack.allocQuaternion();
    getPlaneEquation(plane, i);
    planeNormal.set(plane.x, plane.y, plane.z);
    Vector3 tmp = stack.allocVector3();
    tmp.set(planeNormal).scl(-1);
    localGetSupportingVertex(tmp, planeSupport);
    stack.leave();
}

public void getPlaneEquation(Quaternion plane, int i) {
    Stack stack = Stack.enter();
    Vector3 halfExtents = getHalfExtentsWithoutMargin(stack.allocVector3());

    switch (i) {
        case 0:
            plane.set(1f, 0f, 0f, -halfExtents.x);
            break;
        case 1:
            plane.set(-1f, 0f, 0f, -halfExtents.x);
            break;
        case 2:
            plane.set(0f, 1f, 0f, -halfExtents.y);
            break;
        case 3:
            plane.set(0f, -1f, 0f, -halfExtents.y);
            break;
        case 4:
            plane.set(0f, 0f, 1f, -halfExtents.z);
            break;
        case 5:
            plane.set(0f, 0f, -1f, -halfExtents.z);
            break;
        default:
            assert (false);
    }
    stack.leave();
}

public static int closestAxis4 (Quaternion vec) {
    Quaternion tmp = new Quaternion(vec);
    tmp.x = Math.abs(tmp.x);
    tmp.y = Math.abs(tmp.y);
    tmp.z = Math.abs(tmp.z);
    tmp.w = Math.abs(tmp.w);
    return maxAxis4(tmp);
}

public static void cross3 (Vector3 dest, Quaternion v1, Quaternion v2) {
    float x, y;
    x = v1.y * v2.z - v1.z * v2.y;
    y = v2.x * v1.z - v2.z * v1.x;
    dest.z = v1.x * v2.y - v1.y * v2.x;
    dest.x = x;
    dest.y = y;
}

public static void calculateDiffAxisAngle (Transform transform0, Transform transform1, Vector3 axis, float[] angle) {
// #ifdef USE_QUATERNION_DIFF
// btQuaternion orn0 = transform0.getRotation();
// btQuaternion orn1a = transform1.getRotation();
// btQuaternion orn1 = orn0.farthest(orn1a);
// btQuaternion dorn = orn1 * orn0.inverse();
// #else
        Stack stack = Stack.enter();
        Matrix3 tmp = stack.allocMatrix3();
        tmp.set(transform0.basis);
        MatrixUtil.invert(tmp);

        Matrix3 dmat = stack.allocMatrix3();
        dmat.set(transform1.basis).mul(tmp);

        Quaternion dorn = stack.allocQuaternion();
        MatrixUtil.getRotation(dmat, dorn);
// #endif

        // floating point inaccuracy can lead to w component > 1..., which breaks

        dorn.nor();

        angle[0] = QuaternionUtil.getAngle(dorn);
        axis.set(dorn.x, dorn.y, dorn.z);
        // TODO: probably not needed
        // axis[3] = btScalar(0.);

        // check for axis length
        float len = axis.len2();
        if (len < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
            axis.set(1f, 0f, 0f);
        } else {
            axis.scl(1f / (float)Math.sqrt(len));
        }
        stack.leave();
    }

public static void mul (Quaternion q, Vector3 w) {
    float rx = q.w * w.x + q.y * w.z - q.z * w.y;
    float ry = q.w * w.y + q.z * w.x - q.x * w.z;
    float rz = q.w * w.z + q.x * w.y - q.y * w.x;
    float rw = -q.x * w.x - q.y * w.y - q.z * w.z;
    q.set(rx, ry, rz, rw);
}

public static Vector3 quatRotate (Quaternion rotation, Vector3 v, Vector3 out) {
    Stack stack = Stack.enter();
    Quaternion q = stack.alloc(rotation);
    QuaternionUtil.mul(q, v);

    Quaternion tmp = stack.allocQuaternion();
    inverse(tmp, rotation);
    q.mul(tmp);

    out.set(q.x, q.y, q.z);
    stack.leave();
    return out;
}

public static void setEuler (Quaternion q, float yaw, float pitch, float roll) {
    float halfYaw = yaw * 0.5f;
    float halfPitch = pitch * 0.5f;
    float halfRoll = roll * 0.5f;
    float cosYaw = (float)Math.cos(halfYaw);
    float sinYaw = (float)Math.sin(halfYaw);
    float cosPitch = (float)Math.cos(halfPitch);
    float sinPitch = (float)Math.sin(halfPitch);
    float cosRoll = (float)Math.cos(halfRoll);
    float sinRoll = (float)Math.sin(halfRoll);
    q.x = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
    q.y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
    q.z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
    q.w = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
}

public static boolean isPointInsidePlanes(ObjectArrayList<Quaternion> planeEquations, Vector3 point, float margin) {
    int numbrushes = planeEquations.size();
    for (int i = 0; i < numbrushes; i++) {
        Quaternion N1 = planeEquations.getQuick(i);
        float dist = VectorUtil.dot3(N1, point) + N1.w - margin;
        if (dist > 0f) {
            return false;
        }
    }
    return true;
}

public static boolean areVerticesBehindPlane(Quaternion planeNormal, ObjectArrayList<Vector3> vertices, float margin) {
    int numvertices = vertices.size();
    for (int i = 0; i < numvertices; i++) {
        Vector3 N1 = vertices.getQuick(i);
        float dist = VectorUtil.dot3(planeNormal, N1) + planeNormal.w - margin;
        if (dist > 0f) {
            return false;
        }
    }
    return true;
}

private static boolean notExist(Quaternion planeEquation, ObjectArrayList<Quaternion> planeEquations) {
    int numbrushes = planeEquations.size();
    for (int i = 0; i < numbrushes; i++) {
        Quaternion N1 = planeEquations.getQuick(i);
        if (VectorUtil.dot3(planeEquation, N1) > 0.999f) {
            return false;
        }
    }
    return true;
}

public Quaternion allocQuaternion() {
    types[sp++] = TYPE_QUATERNION;
    int pos = stackPositions[TYPE_QUATERNION]++;
    if (quat4fStack.size() <= pos) {
        quat4fStack.add(new Quaternion());
    }
    return quat4fStack.get(pos);
}