예제 #1
0
        public void Transform(float3 position, Quaternion orientation)
        {
            //   Transforms the plane to the space defined by the
            //   given position/orientation
            float3 newNormal = Quaternion.Inverse(orientation) * normal;
            float3 origin = Quaternion.Inverse(orientation) * (-normal * dist - position);

            normal = newNormal;
            dist = -float3.dot(newNormal, origin);
        }
예제 #2
0
        public static float4x4 MatrixFromQuatVec(Quaternion q, float3 v)
        {
            // builds a 4x4 transformation matrix based on orientation q and translation v
            float qx2 = q.x * q.x;
            float qy2 = q.y * q.y;
            float qz2 = q.z * q.z;

            float qxqy = q.x * q.y;
            float qxqz = q.x * q.z;
            float qxqw = q.x * q.w;
            float qyqz = q.y * q.z;
            float qyqw = q.y * q.w;
            float qzqw = q.z * q.w;

            return new float4x4(
                1 - 2 * (qy2 + qz2),
                2 * (qxqy + qzqw),
                2 * (qxqz - qyqw),
                0,
                2 * (qxqy - qzqw),
                1 - 2 * (qx2 + qz2),
                2 * (qyqz + qxqw),
                0,
                2 * (qxqz + qyqw),
                2 * (qyqz - qxqw),
                1 - 2 * (qx2 + qy2),
                0,
                v.x,
                v.y,
                v.z,
                1.0f);
        }
예제 #3
0
 public static float Roll(Quaternion q)
 {
     q = new Quaternion(new float3(0.0f, 0.0f, 1.0f), -Yaw(q) * (3.14159264f / 180.0f)) * q;
     q = new Quaternion(new float3(1.0f, 0.0f, 0.0f), -Pitch(q) * (3.14159264f / 180.0f)) * q;
     return (float)Math.Atan2(-q.xdir().z, q.xdir().x) * (180.0f / 3.14159264f);
 }
예제 #4
0
 public static Quaternion operator *(Quaternion a, Quaternion b)
 {
     Quaternion c = new Quaternion();
     c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
     c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;
     c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;
     c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;
     return c;
 }
예제 #5
0
 public static float Yaw(Quaternion q)
 {
     float3 v = q.ydir();
     return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f);
 }
예제 #6
0
 public static float Pitch(Quaternion q)
 {
     float3 v = q.ydir();
     return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f);
 }
예제 #7
0
 public static Quaternion Inverse(Quaternion q)
 {
     return new Quaternion(-q.x, -q.y, -q.z, q.w);
 }
예제 #8
0
 public static Quaternion Interpolate(Quaternion q0, Quaternion q1, float alpha)
 {
     return slerp(q0, q1, alpha);
 }
예제 #9
0
 public static Quaternion slerp(Quaternion a, Quaternion b, float interp)
 {
     if (dot(a, b) < 0.0)
     {
         a.w = -a.w;
         a.x = -a.x;
         a.y = -a.y;
         a.z = -a.z;
     }
     float d = dot(a, b);
     if (d >= 1.0)
     {
         return a;
     }
     float theta = (float)Math.Acos(d);
     if (theta == 0.0f)
     {
         return (a);
     }
     return a * ((float)Math.Sin(theta - interp * theta) / (float)Math.Sin(theta)) + b * ((float)Math.Sin(interp * theta) / (float)Math.Sin(theta));
 }
예제 #10
0
 public static float dot(Quaternion a, Quaternion b)
 {
     return (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z);
 }
예제 #11
0
 public static Quaternion normalize(Quaternion a)
 {
     float m = (float)Math.Sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z);
     if (m < 0.000000001f)
     {
         a.w = 1;
         a.x = a.y = a.z = 0;
         return a;
     }
     return a * (1f / m);
 }