quaternion.java

使用stl技术,(还没看,是听说的)

/*  Ogre4J
    Copyright (C) 2002 macross

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/
package org.ogre4j.math;

import java.io.Serializable;

/**
 * Quaternion
 * 
 * @author Ivica Aracic <[e-mail]>
 */
public class Quaternion implements Serializable {
    
    public static final float 
        ms_fEpsilon = 1e-03f;
    
    public static final Quaternion            
        ZERO     = new Quaternion(0,0,0,0),
        IDENTITY = new Quaternion(1,0,0,0);
    
    public float w, x,y,z;
    
    public Quaternion(float w, float x, float y, float z) {
        set(w, x,y,z);
    }

    public Quaternion(Quaternion other) {
        set(other.w, other.x,other.y,other.z);
    }
    
    public void set(float w, float x, float y, float z) {
        this.w = w;
        this.x = x;
        this.y = y;
        this.z = z;
        
    }
    
    
    
    public void fromAngleAxis(float rfAngle, Vector3 rkAxis) {
        // assert:  axis[] is unit length
        //
        // The quaternion representing the rotation is
        //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

        float fHalfAngle = 0.5f*rfAngle;
        float fSin = (float)Math.sin(fHalfAngle);
        w = (float)Math.cos(fHalfAngle);
        x = fSin*rkAxis.x;
        y = fSin*rkAxis.y;
        z = fSin*rkAxis.z;
    }
    //-----------------------------------------------------------------------
    public void toAngleAxis (float rfAngle, Vector3 rkAxis) {
        // The quaternion representing the rotation is
        //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

        float fSqrLength = x*x+y*y+z*z;
        if ( fSqrLength > 0.0 )
        {
            rfAngle = 2.0f*(float)Math.acos(w);
            float fInvLength = 1.0f/(float)Math.sqrt(fSqrLength);
            rkAxis.x = x*fInvLength;
            rkAxis.y = y*fInvLength;
            rkAxis.z = z*fInvLength;
        }
        else
        {
            // angle is 0 (mod 2*pi), so any axis will do
            rfAngle = 0.0f;
            rkAxis.x = 1.0f;
            rkAxis.y = 0.0f;
            rkAxis.z = 0.0f;
        }
    }
    //-----------------------------------------------------------------------
        
    public Quaternion add(Quaternion rkQ) {
        return new Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
    }
    //-----------------------------------------------------------------------
    public Quaternion sub(Quaternion rkQ) {
        return new Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
    }
    //-----------------------------------------------------------------------
    public Quaternion multQ(Quaternion rkQ) {
        // NOTE:  Multiplication is not generally commutative, so in most
        // cases p*q != q*p.

        return
            new Quaternion(
                w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
                w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
                w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
                w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
            );
    }
    //-----------------------------------------------------------------------
    public Quaternion mult(float fScalar) {
        return new Quaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
    }
    //-----------------------------------------------------------------------
    public Quaternion mult(float fScalar, Quaternion rkQ) {
        return new Quaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
            fScalar*rkQ.z);
    }
    //-----------------------------------------------------------------------
    public Quaternion neg() {
        return new Quaternion(-w,-x,-y,-z);
    }
    //-----------------------------------------------------------------------
    public float dot(Quaternion rkQ) {
        return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
    }
    //-----------------------------------------------------------------------
    public float norm() {
        return w*w+x*x+y*y+z*z;
    }
    //-----------------------------------------------------------------------
    public Quaternion inverse() {
        float fNorm = w*w+x*x+y*y+z*z;
        if ( fNorm > 0.0 ) {
            float fInvNorm = 1.0f/fNorm;
            return new Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
        }
        else
        {
            // return an invalid result to flag the error
            return ZERO;
        }
    }
    //-----------------------------------------------------------------------
    public Quaternion unitInverse() {
        // assert:  'this' is unit length
        return new Quaternion(w,-x,-y,-z);
    }    
    
    //-----------------------------------------------------------------------
    public static Quaternion Slerp(float fT, Quaternion rkP, Quaternion rkQ) {
        float fCos = rkP.dot(rkQ);
        if(fCos>1.0f) fCos = 1.0f;
        float fAngle = (float)Math.acos(fCos);

        if(Math.abs(fAngle) < ms_fEpsilon) {
            return new Quaternion(rkP);
        }

        float fSin = (float)Math.sin(fAngle);
        float fInvSin = 1.0f/fSin;
        float fCoeff0 = (float)Math.sin((1.0f-fT)*fAngle)*fInvSin;
        float fCoeff1 = (float)Math.sin(fT*fAngle)*fInvSin;
        
        return
            rkP.mult(fCoeff0).add(rkQ.mult(fCoeff1));
    }

    //-----------------------------------------------------------------------
    public boolean isEqual(Quaternion rhs) {
        return (rhs.x == x) && (rhs.y == y) &&
            (rhs.z == z) && (rhs.w == w);
    }
    
    public String toString() {
        return "("+w+","+x+","+y+","+z+") norm = "+norm();
    }
}