Quaternion.java

quaternion-java.png
package miffy.sora;
 
import java.nio.FloatBuffer;
 
public class Quaternion {
   private float w;
   private float x;
   private float y;
   private float z;
   public Quaternion(){//Constructor
       w=1.0f;
       x=0.0f;
       y=0.0f;
       z=0.0f;
   }
   public Quaternion(float _w,float _x,float _y,float _z){//Constructor
       w=_w;
       x=_x;
       y=_y;
       z=_z;
   }
   public Quaternion(Quaternion _in)
   {
       w=_in.w;
       x=_in.x;
       y=_in.y;
       z=_in.z;
   }
   // Quaternionの積
   public Quaternion multiply( Quaternion _in)
   {
       Quaternion copied = new Quaternion(this);
       w=copied.w * _in.w - copied.x * _in.x - copied.y * _in.y - copied.z * _in.z;
       x=copied.w * _in.x + copied.x * _in.w + copied.y * _in.z - copied.z * _in.y;
       y=copied.w * _in.y - copied.x * _in.z + copied.y * _in.w + copied.z * _in.x;
       z=copied.w * _in.z + copied.x * _in.y - copied.y * _in.x + copied.z * _in.w;
 
       return this;
   }
   public String GetString(){
       String ret="w="+w+",x="+x+",y="+y+",z="+z;
 
       return ret;
 
   }
   //クォータニオンから回転行列を算出
   void GetRotationMatrix(float[] _r )
   {
       float xx = x * x * 2.0f;
       float yy = y * y * 2.0f;
       float zz = z * z * 2.0f;
       float xy = x * y * 2.0f;
       float yz = y * z * 2.0f;
       float zx = z * x * 2.0f;
       float xw = x * w * 2.0f;
       float yw = y * w * 2.0f;
       float zw = z * w * 2.0f;
       float[] r1={
           1.0f - yy - zz, xy + zw, zx - yw, 0.0f,
           xy - zw, 1.0f - zz - xx, yz + xw, 0.0f,
           zx + yw, yz - xw, 1.0f - xx - yy, 0.0f,
           0.0f, 0.0f, 0.0f, 1.0f
       };
       for (int i = 0;i < _r.length;i++) {
           _r[i]=r1[i];
       }
   }
   //クォータニオンから回転行列を算出
       void GetRotationMatrixBuf(FloatBuffer _r )
       {
           float xx = x * x * 2.0f;
           float yy = y * y * 2.0f;
           float zz = z * z * 2.0f;
           float xy = x * y * 2.0f;
           float yz = y * z * 2.0f;
           float zx = z * x * 2.0f;
           float xw = x * w * 2.0f;
           float yw = y * w * 2.0f;
           float zw = z * w * 2.0f;
 
           _r.put(0,1.0f - yy - zz);
           _r.put(1,xy + zw);
           _r.put(2,zx - yw);
           _r.put(3,0.0f);
 
           _r.put(4,xy - zw);
           _r.put(5,1.0f - zz - xx);
           _r.put(6,yz + xw);
           _r.put(7,0.0f);
           _r.put(8,zx + yw);
           _r.put(9,yz - xw);
           _r.put(10,1.0f - xx - yy);
           _r.put(11,0.0f);
 
           _r.put(12,0.0f);
           _r.put(13,0.0f);
           _r.put(14,0.0f);
           _r.put(15,1.0f);
 
       }
}

サポートサイト Wikidot.com