Exemple #1
0
        public static tfloat Determinant(Matrix44 m)
        {
            tfloat v1 = m.m22 * m.m33 - m.m23 * m.m32;
            tfloat v2 = m.m23 * m.m31 - m.m21 * m.m33;
            tfloat v3 = m.m21 * m.m32 - m.m22 * m.m31;

            tfloat t1 = m.m11 * v1;
            tfloat t2 = m.m12 * v2;
            tfloat t3 = m.m13 * v3;
            tfloat x1 = m.m00 * (t1 + t2 + t3);

            tfloat v4 = m.m23 * m.m30 - m.m20 * m.m33;
            tfloat v5 = m.m20 * m.m32 - m.m22 * m.m30;

            t1 = m.m10 * v1;
            t2 = m.m12 * v4;
            t3 = m.m13 * v5;
            tfloat x2 = -m.m01 * (t1 + t2 + t3);

            tfloat v6 = m.m20 * m.m31 - m.m21 * m.m30;

            t1 = m.m10 * (-v2);
            t2 = m.m11 * v4;
            t3 = m.m13 * v6;
            tfloat x3 = m.m02 * (t1 + t2 + t3);

            t1 = m.m10 * v3;
            t2 = m.m11 * (-v5);
            t3 = m.m12 * v6;
            tfloat x4 = -m.m03 * (t1 + t2 + t3);

            return(x1 + x2 + x3 + x4);
        }
Exemple #2
0
 public                         tfloat this [int index] {
     get {
         if (index == 0)
         {
             return(this.x);
         }
         if (index == 1)
         {
             return(this.y);
         }
         throw new IndexOutOfRangeException("Vector2 index Invalid");
     }
     set {
         if (index == 0)
         {
             this.x = value;
         }
         else if (index == 1)
         {
             this.y = value;
         }
         else
         {
             throw new IndexOutOfRangeException("Vector2 index Invalid");
         }
     }
 }
Exemple #3
0
 public void Scale(Vector4 scale)
 {
     this.x *= scale.x;
     this.y *= scale.y;
     this.z *= scale.z;
     this.w *= scale.w;
 }
Exemple #4
0
        private static Vector3 MakePositive(Vector3 degrees)
        {
            tfloat negativeFlip = -0.0001f * Math.RAD2DEG;
            tfloat positiveFlip = 360.0f + negativeFlip;

            if (degrees.x < negativeFlip)
            {
                degrees.x += 360.0f;
            }
            else if (degrees.x > positiveFlip)
            {
                degrees.x -= 360.0f;
            }

            if (degrees.y < negativeFlip)
            {
                degrees.y += 360.0f;
            }
            else if (degrees.y > positiveFlip)
            {
                degrees.y -= 360.0f;
            }

            if (degrees.z < negativeFlip)
            {
                degrees.z += 360.0f;
            }
            else if (degrees.z > positiveFlip)
            {
                degrees.z -= 360.0f;
            }

            return(degrees);
        }
Exemple #5
0
 public Vector4(tfloat x, tfloat y)
 {
     this.x = x;
     this.y = y;
     this.z = Math.ZERO;
     this.w = Math.ZERO;
 }
Exemple #6
0
        /// <summary>
        /// Input euler angle is rad
        /// </summary>
        /// <param name="someEulerAngles"></param>
        /// <param name="order"></param>
        /// <returns></returns>
        private static Quaternion EulerToQuaternion(Vector3 someEulerAngles,
                                                    RotationOrder order = RotationOrder.kOrderUnityDefault)
        {
            tfloat cX = qCos(someEulerAngles.x / 2.0f);
            tfloat sX = qSin(someEulerAngles.x / 2.0f);

            tfloat cY = qCos(someEulerAngles.y / 2.0f);
            tfloat sY = qSin(someEulerAngles.y / 2.0f);

            tfloat cZ = qCos(someEulerAngles.z / 2.0f);
            tfloat sZ = qSin(someEulerAngles.z / 2.0f);

            Quaternion qX  = new Quaternion(sX, 0.0F, 0.0F, cX);
            Quaternion qY  = new Quaternion(0.0F, sY, 0.0F, cY);
            Quaternion qZ  = new Quaternion(0.0F, 0.0F, sZ, cZ);
            Quaternion ret = Quaternion.identity;

            switch (order)
            {
            case RotationOrder.kOrderZYX: CreateQuaternionFromAxisQuaternions(qX, qY, qZ, out ret); break;

            case RotationOrder.kOrderYZX: CreateQuaternionFromAxisQuaternions(qX, qZ, qY, out ret); break;

            case RotationOrder.kOrderXZY: CreateQuaternionFromAxisQuaternions(qY, qZ, qX, out ret); break;

            case RotationOrder.kOrderZXY: CreateQuaternionFromAxisQuaternions(qY, qX, qZ, out ret); break;

            case RotationOrder.kOrderYXZ: CreateQuaternionFromAxisQuaternions(qZ, qX, qY, out ret); break;

            case RotationOrder.kOrderXYZ: CreateQuaternionFromAxisQuaternions(qZ, qY, qX, out ret); break;
            }

            return(ret);
        }
Exemple #7
0
 public Quaternion(Quaternion qua)
 {
     this.x = qua.x;
     this.y = qua.y;
     this.z = qua.z;
     this.w = qua.w;
 }
Exemple #8
0
 public Vector4(tfloat x, tfloat y, tfloat z, tfloat w)
 {
     this.x = x;
     this.y = y;
     this.z = z;
     this.w = w;
 }
Exemple #9
0
 public void Set(tfloat x, tfloat y, tfloat z, tfloat w)
 {
     this.x = x;
     this.y = y;
     this.z = z;
     this.w = w;
 }
Exemple #10
0
 public void Set(float inX, float inY, float inZ, float inW)
 {
     x = inX;
     y = inY;
     z = inZ;
     w = inW;
 }
Exemple #11
0
 public Quaternion(tfloat x, tfloat y, tfloat z, tfloat w)
 {
     this.x = x;
     this.y = y;
     this.z = z;
     this.w = w;
 }
Exemple #12
0
 public static Vector3 ClampMagnitude(Vector3 vector, tfloat maxLength)
 {
     if (vector.sqrMagnitude > maxLength * maxLength)
     {
         return(vector.normalized * maxLength);
     }
     return(vector);
 }
Exemple #13
0
        public void FromToRotation(Vector3 from, Vector3 to, ref Matrix33 mtx)
        {
            Vector3 v = Vector3.Cross(from, to);
            tfloat  e = Vector3.Dot(from, to);

            if (e > (Math.ONE - Math.Epsilon))
            {
                mtx[0, 0] = Math.ONE; mtx[0, 1] = Math.ZERO; mtx[0, 2] = Math.ZERO;
                mtx[0, 0] = Math.ZERO; mtx[0, 1] = Math.ONE; mtx[0, 2] = Math.ZERO;
                mtx[0, 0] = Math.ZERO; mtx[0, 1] = Math.ZERO; mtx[0, 2] = Math.ONE;
            }
            else if (e < (new tfloat(-1) * Math.ONE + Math.Epsilon))
            {
                Vector3 up, left = Vector3.ZERO;
                tfloat  invlen;
                tfloat  fxx, fyy, fzz, fxy, fxz, fyz;
                tfloat  uxx, uyy, uzz, uxy, uxz, uyz;
                tfloat  lxx, lyy, lzz, lxy, lxz, lyz;

                left[0] = Math.ZERO; left[1] = from[2]; left[2] = -from[1];
                if (Vector3.Dot(left, left) < Math.Epsilon)
                {
                    left[0] = -from[2]; left[1] = Math.ZERO; left[2] = from[0];
                }

                invlen   = Math.ONE / Math.Sqrt(Vector3.Dot(left, left));
                left[0] *= invlen;
                left[1] *= invlen;
                left[2] *= invlen;
                up       = Vector3.Cross(left, from);

                fxx = -from[0] * from[0]; fyy = -from[1] * from[1]; fzz = -from[2] * from[2];
                fxy = -from[0] * from[1]; fxz = -from[0] * from[2]; fyz = -from[1] * from[2];

                uxx = up[0] * up[0]; uyy = up[1] * up[1]; uzz = up[2] * up[2];
                uxy = up[0] * up[1]; uxz = up[0] * up[2]; uyz = up[1] * up[2];

                lxx = -left[0] * left[0]; lyy = -left[1] * left[1]; lzz = -left[2] * left[2];
                lxy = -left[0] * left[1]; lxz = -left[0] * left[2]; lyz = -left[1] * left[2];
                /* symmetric matrix */
                mtx[0, 0] = fxx + uxx + lxx; mtx[0, 1] = fxy + uxy + lxy; mtx[0, 2] = fxz + uxz + lxz;
                mtx[1, 0] = mtx[0, 1]; mtx[1, 1] = fyy + uyy + lyy; mtx[1, 2] = fyz + uyz + lyz;
                mtx[2, 0] = mtx[0, 2]; mtx[2, 1] = mtx[1, 2]; mtx[2, 2] = fzz + uzz + lzz;
            }
            else
            {
                tfloat hvx, hvz, hvxy, hvxz, hvyz;
                tfloat h = (Math.ONE - e) / Vector3.Dot(v, v);
                hvx       = h * v[0];
                hvz       = h * v[2];
                hvxy      = hvx * v[1];
                hvxz      = hvx * v[2];
                hvyz      = hvz * v[1];
                mtx[0, 0] = e + hvx * v[0]; mtx[0, 1] = hvxy - v[2]; mtx[0, 2] = hvxz + v[1];
                mtx[1, 0] = hvxy + v[2]; mtx[1, 1] = e + h * v[1] * v[1]; mtx[1, 2] = hvyz - v[0];
                mtx[2, 0] = hvxz - v[1]; mtx[2, 1] = hvyz + v[0]; mtx[2, 2] = e + hvz * v[2];
            }
        }
Exemple #14
0
        public static Quaternion operator *(Quaternion lhs, Quaternion rhs)
        {
            tfloat     tempx  = lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y;
            tfloat     tempy  = lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z;
            tfloat     tempz  = lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x;
            tfloat     tempw  = lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z;
            Quaternion result = new Quaternion(tempx, tempy, tempz, tempw);

            return(result);
        }
Exemple #15
0
        public static Vector4 Normalize(Vector4 a)
        {
            tfloat num = Vector4.Magnitude(a);

            if (num > Math.Epsilon)
            {
                return(a / num);
            }
            return(Vector4.ZERO);
        }
Exemple #16
0
        // Return angle in degree [0 - 360]
        public static tfloat ToDegAngle(tfloat y, tfloat x)
        {
            tfloat angle = Atan2(y, x) * Rad2Deg;

            if (angle < 0)
            {
                angle += 360;
            }
            return(angle);
        }
Exemple #17
0
        public static Vector3 Project(Vector3 vector, Vector3 onNormal)
        {
            tfloat num = Vector3.Dot(onNormal, onNormal);

            if (num < Math.Epsilon)
            {
                return(Vector3.ZERO);
            }
            return(onNormal * Vector3.Dot(vector, onNormal) / num);
        }
Exemple #18
0
        public static Vector3 Normalize(Vector3 value)
        {
            tfloat num = Vector3.Magnitude(value);

            if (num > Math.Epsilon)
            {
                return(value / num);
            }
            return(Vector3.ZERO);
        }
Exemple #19
0
        public static Vector4 MoveTowards(Vector4 current, Vector4 target, tfloat maxDistanceDelta)
        {
            Vector4 a         = target - current;
            tfloat  magnitude = a.magnitude;

            if (magnitude <= maxDistanceDelta || magnitude == Math.ZERO)
            {
                return(target);
            }
            return(current + a / magnitude * maxDistanceDelta);
        }
Exemple #20
0
        public void Normalize()
        {
            tfloat magnitude = this.magnitude;

            if (magnitude > Math.Epsilon)
            {
                this /= magnitude;
            }
            else
            {
                this = Vector2.ZERO;
            }
        }
Exemple #21
0
        public static Matrix44 Perspective(tfloat fov, tfloat aspect, tfloat zNear, tfloat zFar)
        {
            Matrix44 result    = new Matrix44();
            tfloat   radians   = Math.DEG2RAD * (fov / Math.TWO);
            tfloat   cotangent = Math.Cos(radians) / Math.Sin(radians);
            tfloat   deltaZ    = zNear - zFar;

            result [0, 0] = cotangent / aspect;      result [0, 1] = Math.ZERO;      result [0, 2] = Math.ZERO;                    result [0, 3] = Math.ZERO;
            result [1, 0] = Math.ZERO;               result [1, 1] = cotangent;      result [1, 2] = Math.ZERO;                    result [1, 3] = Math.ZERO;
            result [2, 0] = Math.ZERO;               result [2, 1] = Math.ZERO;      result [2, 2] = (zFar + zNear) / deltaZ;      result [2, 3] = Math.TWO * zNear * zFar / deltaZ;
            result [3, 0] = Math.ZERO;               result [3, 1] = Math.ZERO;      result [3, 2] = -Math.ONE;                    result [3, 3] = Math.ZERO;
            return(result);
        }
Exemple #22
0
        public void Normalize()
        {
            tfloat num = Vector4.Magnitude(this);

            if (num > Math.Epsilon)
            {
                this /= num;
            }
            else
            {
                this = Vector4.ZERO;
            }
        }
Exemple #23
0
        public static Quaternion NormalizeSafe(ref Quaternion q)
        {
            tfloat mag = Magnitude(ref q);

            if (mag < Math.Epsilon)
            {
                return(Quaternion.identity);
            }
            else
            {
                return(q / mag);
            }
        }
Exemple #24
0
        public Vector3 MultiplyPoint(Vector3 v)
        {
            Vector3 result;

            result.x = this.m00 * v.x + this.m01 * v.y + this.m02 * v.z + this.m03;
            result.y = this.m10 * v.x + this.m11 * v.y + this.m12 * v.z + this.m13;
            result.z = this.m20 * v.x + this.m21 * v.y + this.m22 * v.z + this.m23;
            tfloat num = this.m30 * v.x + this.m31 * v.y + this.m32 * v.z + this.m33;

            num       = Math.ONE / num;
            result.x *= num;
            result.y *= num;
            result.z *= num;
            return(result);
        }
Exemple #25
0
 public static tfloat Clamp01(tfloat a)
 {
     if (a > 1.0f)
     {
         return(1.0f);
     }
     else if (a < 0.0f)
     {
         return(0.0f);
     }
     else
     {
         return(a);
     }
 }
Exemple #26
0
        public                            tfloat this[int index]
        {
            get
            {
                switch (index)
                {
                case 0:
                    return(this.x);

                case 1:
                    return(this.y);

                case 2:
                    return(this.z);

                case 3:
                    return(this.w);

                default:
                    throw new IndexOutOfRangeException("Invalid Quaternion index!");
                }
            }
            set
            {
                switch (index)
                {
                case 0:
                    this.x = value;
                    break;

                case 1:
                    this.y = value;
                    break;

                case 2:
                    this.z = value;
                    break;

                case 3:
                    this.w = value;
                    break;

                default:
                    throw new IndexOutOfRangeException("Invalid Quaternion index!");
                }
            }
        }
Exemple #27
0
        public static Matrix44 Ortho(tfloat left, tfloat right, tfloat bottom, tfloat top, tfloat zNear, tfloat zFar)
        {
            Matrix44 result = identity;

            tfloat deltax = right - left;
            tfloat deltay = top - bottom;
            tfloat deltaz = zFar - zNear;

            result[0, 0] = Math.TWO / deltax;
            result[0, 3] = -(right + left) / deltax;
            result[1, 1] = Math.TWO / deltay;
            result[1, 3] = -(top + bottom) / deltay;
            result[2, 2] = -Math.TWO / deltaz;
            result[2, 3] = -(zFar + zNear) / deltaz;

            return(result);
        }
Exemple #28
0
        public static void MatrixToQuaternion(Matrix33 mtx, ref Quaternion q)
        {
            tfloat fTrace = mtx[0, 0] + mtx[1, 1] + mtx[2, 2];
            tfloat fRoot;

            if (fTrace > Math.ZERO)
            {
                fRoot = Math.Sqrt(fTrace + Math.ONE); // 2w
                q.w   = 0.5f * fRoot;
                fRoot = 0.5f / fRoot;                 // 1/(4w)
                q.x   = (mtx[2, 1] - mtx[1, 2]) * fRoot;
                q.y   = (mtx[0, 2] - mtx[2, 0]) * fRoot;
                q.z   = (mtx[1, 0] - mtx[0, 1]) * fRoot;
            }
            else
            {
                int[] next = new int[3] {
                    1, 2, 0
                };

                int i = 0;
                if (mtx[1, 1] > mtx[0, 0])
                {
                    i = 1;
                }

                if (mtx[2, 2] > mtx[i, i])
                {
                    i = 2;
                }

                int j = next[i];
                int k = next[j];

                fRoot = Math.Sqrt(mtx[i, i] - mtx[j, j] - mtx[k, k] + Math.ONE);
                q[i]  = 0.5f / fRoot;
                q.w   = (mtx[k, j] - mtx[j, k]) * fRoot;
                q[j]  = (mtx[j, i] + mtx[i, j]) * fRoot;
                q[k]  = (mtx[k, i] + mtx[i, k]) * fRoot;
            }

            q = Normalize(q);
        }
Exemple #29
0
        public static Vector3 operator *(Quaternion rotation, Vector3 point)
        {
            tfloat  num   = rotation.x * Math.TWO;
            tfloat  num2  = rotation.y * Math.TWO;
            tfloat  num3  = rotation.z * Math.TWO;
            tfloat  num4  = rotation.x * num;
            tfloat  num5  = rotation.y * num2;
            tfloat  num6  = rotation.z * num3;
            tfloat  num7  = rotation.x * num2;
            tfloat  num8  = rotation.x * num3;
            tfloat  num9  = rotation.y * num3;
            tfloat  num10 = rotation.w * num;
            tfloat  num11 = rotation.w * num2;
            tfloat  num12 = rotation.w * num3;
            Vector3 result;

            result.x = (Math.ONE - (num5 + num6)) * point.x + (num7 - num12) * point.y + (num8 + num11) * point.z;
            result.y = (num7 + num12) * point.x + (Math.ONE - (num4 + num6)) * point.y + (num9 - num10) * point.z;
            result.z = (num8 - num11) * point.x + (num9 + num10) * point.y + (Math.ONE - (num4 + num5)) * point.z;
            return(result);
        }
Exemple #30
0
        public static Quaternion FromToRotation(Vector3 fromDirection, Vector3 toDirection)
        {
            tfloat lhsMag = fromDirection.magnitude;
            tfloat rhsMag = toDirection.magnitude;

            if (lhsMag < Math.Epsilon || rhsMag < Math.Epsilon)
            {
                return(identity);
            }

            //nomarl///
            Vector3 lhs = fromDirection / lhsMag;
            Vector3 rhs = toDirection / rhsMag;

            Matrix33 mtx = new Matrix33();

            mtx = mtx.SetFromToRotation(lhs, rhs);

            Quaternion q = new Quaternion();

            MatrixToQuaternion(mtx, ref q);
            return(q);
        }