Esempio n. 1
0
 public Matrix3f(Matrix3f rm)
 {
     for (int i = 0; i < 9; i++)
     {
         m_elements[i] = rm.getElement(i);
     }
 }
Esempio n. 2
0
    //TODO: cross linear and rotational
    public void Integrate()
    {
        //linear
        //set position first
        ns_position = position + m_linearVelocity * Time.fixedDeltaTime;
        //set the velocity
        ns_m_linearVelocity = m_linearVelocity + inv_mass * this.forces * Time.fixedDeltaTime;



        //set the acceleration
        //ns_m_linearAcceleration += forces * inv_mass;



        //rotational

        GK.Matrix3f skewedAngurlar = new GK.Matrix3f();
        skewedAngurlar = skewedAngurlar.SkewSymmetry(m_AngularVelocity);

        ns_orientation = orientation + skewedAngurlar * Time.fixedDeltaTime * orientation;
        Debug.Log("Orientation");
        ns_orientation.print();

        ns_AngularMomentum = AngularMomentum + Time.fixedDeltaTime * Torque;

        GK.Matrix3f transposedOrientation = new GK.Matrix3f();
        transposedOrientation        = ns_orientation.Transposed();
        ns_WorldInertiaInverseTensor = (ns_orientation * BodyInertiaInverseTensor) * transposedOrientation;
        Debug.Log("WorldInertia");
        ns_WorldInertiaInverseTensor.print();
        ns_orientation.OrthonormalizeOrientation();

        ns_m_AngularVelocity = ns_WorldInertiaInverseTensor * ns_AngularMomentum;
    }
Esempio n. 3
0
    // Use this for initialization
    //define the intital values here

    public RigidBody()
    {
        inv_mass = 1.0f / this.Mass;
        //position = transform.position;
        orientation       = new GK.Matrix3f();
        m_linearVelocity  = new Vector3();
        m_AngularVelocity = new Vector3();
        Torque            = new Vector3();
        AngularMomentum   = new Vector3();
        forces            = new Vector3();
        //LinearMomentum = new Vector3();
        m_AngularAccelration      = new Vector3();
        WorldInertiaInverseTensor = new GK.Matrix3f();
        BodyInertiaInverseTensor  = new GK.Matrix3f();

        //position = transform.position;
        ns_orientation       = new GK.Matrix3f();
        ns_m_linearVelocity  = new Vector3();
        ns_m_AngularVelocity = new Vector3();
        ns_Torque            = new Vector3();
        ns_AngularMomentum   = new Vector3();
        ns_forces            = new Vector3();
        //ns_LinearMomentum = new Vector3();
        ns_m_AngularAccelration      = new Vector3();
        ns_WorldInertiaInverseTensor = new GK.Matrix3f();
    }
Esempio n. 4
0
        public bool intersect(MRay ray, MHit hit, float tmin)
        {
            Vector3 R0 = ray.origin;
            Vector3 Rd = ray.direction;
            Vector3 e1 = b - a;
            Vector3 e2 = c - a;
            //R0-a=beta*e1+gamma*e2-t*Rd
            Matrix3f A        = new Matrix3f(-e1, -e2, Rd, true);
            Matrix3f betaM    = new Matrix3f(a - R0, -e2, Rd, true);
            Matrix3f gammaM   = new Matrix3f(-e1, a - R0, Rd, true);
            Matrix3f tM       = new Matrix3f(-e1, -e2, a - R0, true);
            float    beta     = betaM.determinant() / A.determinant();
            float    gamma    = gammaM.determinant() / A.determinant();
            float    result_t = tM.determinant() / A.determinant();
            float    alpha    = 1.0f - beta - gamma;

            if (beta + gamma > 1 || beta < 0 || gamma < 0)
            {
                return(false);
            }
            if (result_t >= tmin && result_t < hit.t)
            {
                Vector3 newNormal = (alpha * normals[0] + beta * normals[1] + gamma * normals[2]).normalized;
                hit.set(result_t, newNormal);
                return(true);
            }

            return(false);
        }
Esempio n. 5
0
        //code adapted from https://www.learnopencv.com/rotation-matrix-to-euler-angles/
        public Matrix3f QuaterniontoMatrix3(Quaternion q)
        {
            Matrix3f m   = new Matrix3f();
            float    sqw = q.w * q.w;
            float    sqx = q.x * q.x;
            float    sqy = q.y * q.y;
            float    sqz = q.z * q.z;

            float inverse = 1 / (sqw + sqy + sqz + sqw);

            m.setMELEMENT(0, 0, (sqx - sqy - sqz + sqw) * inverse);
            m.setMELEMENT(1, 1, (-sqx + sqy - sqz + sqw) * inverse);
            m.setMELEMENT(2, 2, (-sqx - sqy + sqz + sqw) * inverse);

            float temp1 = q.x * q.y;
            float temp2 = q.z * q.w;

            m.setMELEMENT(1, 0, 2.0f * (temp1 + temp2) * inverse);
            m.setMELEMENT(0, 1, 2.0f * (temp1 - temp2) * inverse);

            float temp3 = q.x * q.z;
            float temp4 = q.y * q.w;

            m.setMELEMENT(2, 0, 2.0f * (temp3 - temp4) * inverse);
            m.setMELEMENT(0, 2, 2.0f * (temp1 + temp2) * inverse);

            float temp5 = q.y * q.z;
            float temp6 = q.x * q.w;

            m.setMELEMENT(2, 1, 2.0f * (temp5 + temp6) * inverse);
            m.setMELEMENT(1, 2, 2.0f * (temp5 - temp6) * inverse);

            return(m);
        }
Esempio n. 6
0
        public Matrix3f SkewSymmetry(Vector3 v)
        {
            Matrix3f m = new Matrix3f();

            m.m_elements[GetIndex(0, 0)] = 0.0f; m.m_elements[GetIndex(0, 1)] = -v.z; m.m_elements[GetIndex(0, 2)] = v.y;
            m.m_elements[GetIndex(1, 0)] = v.z; m.m_elements[GetIndex(1, 1)] = 0.0f; m.m_elements[GetIndex(1, 2)] = -v.x;
            m.m_elements[GetIndex(2, 0)] = -v.y; m.m_elements[GetIndex(2, 1)] = v.x; m.m_elements[GetIndex(2, 2)] = 0.0f;

            return(m);
        }
Esempio n. 7
0
        public Matrix3f Identity()
        {
            Matrix3f m = new Matrix3f();

            m.getElements()[GetIndex(0, 0)] = 1.0f;
            m.getElements()[GetIndex(1, 1)] = 1.0f;
            m.getElements()[GetIndex(2, 2)] = 1.0f;

            return(m);
        }
Esempio n. 8
0
        // static
        public Matrix3f Ones()
        {
            Matrix3f m = new Matrix3f();

            for (int i = 0; i < 9; ++i)
            {
                m.getElements()[i] = 1;
            }

            return(m);
        }
Esempio n. 9
0
//		public void CollisionResponce(Rigidbody grb,float e=0.5F) {
//			//coefficient of restitution: ratio of speed after/before
//
//
//			//
//			//			vector_3 Velocity = Configuration.CMVelocity +
//			//				CrossProduct(Configuration.AngularVelocity,R);
//			//
//			//			real ImpulseNumerator = -(r(1) + Body.CoefficientOfRestitution) *
//			//				DotProduct(Velocity,CollisionNormal);
//			//
//			//			real ImpulseDenominator = Body.OneOverMass +
//			//				DotProduct(CrossProduct(Configuration.InverseWorldInertiaTensor *
//			//					CrossProduct(R,CollisionNormal),R),
//			//					CollisionNormal);
//			//
//			//			vector_3 Impulse = (ImpulseNumerator/ImpulseDenominator) * CollisionNormal;
//			//
//			//			// apply impulse to primary quantities
//			//			Configuration.CMVelocity += Body.OneOverMass * Impulse;
//			//			Configuration.AngularMomentum += CrossProduct(R,Impulse);
//			//
//			//			// compute affected auxiliary quantities
//			//			Configuration.AngularVelocity = Configuration.InverseWorldInertiaTensor *
//			//				Configuration.AngularMomentum;
//
//			float ma = grb.mass;
//			//manually set first
//
//			Vector3 n = hitNormal;
//
//			//			grb.velocity =Vector3.zero;
//			//			grb.angularVelocity = Vector3.zero;
//			//			grb.Sleep ();
//
//			//			print (grb.velocity);
//
//			Vector3 vai = grb.velocity;
//
//			Vector3 wai = grb.angularVelocity;
//
//			Vector3 ita = grb.inertiaTensor;
//
//			Quaternion itra = grb.inertiaTensorRotation;
//
//			Matrix3f Qa = Matrix3f.rotation (grb.inertiaTensorRotation);
//			Matrix3f QaT = Qa.Transposed ();
//
//
//			Matrix3f Ia = new Matrix3f (ita);
//			//			print (Aa+" ita:"+ita+" itra:"+itra+" Qa:"+Qa+" QaT:"+QaT);
//			//			Matrix3f Ia = Qa * Aa * QaT;
//			Ia.print();
//			Matrix3f IaInverse = Ia.inverse();
//			print ("Pos:"+grb.position+" CM:" + grb.centerOfMass);
//
//			Vector3 ra = grb.position - grb.worldCenterOfMass;
//			Vector3 velocity = vai + Vector3.Cross (wai, ra);
//			float impulsenumerator = -(e + 1) * Vector3.Dot (velocity, n);
//			float impulsedenominator = 1 / ma + Vector3.Dot (Vector3.Cross (IaInverse * Vector3.Cross (ra, n), ra), n);
//			Vector3 J = (impulsenumerator / impulsedenominator) * n;
//			Vector3 vaii = grb.velocity;
//			print ((1 / ma) * J);
//			grb.velocity += (1/ma)*J;
//			grb.angularVelocity = IaInverse * Vector3.Cross (ra,J);
//			Debug.Log ("initial is"+vaii+"final is"+grb.velocity);
//
//
//
//
//			////			IaInverse.print ();
//			//		Vector3 normal = n.normalized;
//			//		Vector3 angularVelChangea  = normal; // start calculating the change in abgular rotation of a
//			//		angularVelChangea=Vector3.Cross(angularVelChangea,ra);
//			//
//			//		Vector3 vaLinDueToR = Vector3.Cross(IaInverse*angularVelChangea, ra);  // calculate the linear velocity of collision point on a due to rotation of a
//			//		float scalar = 1/ma + Vector3.Dot(vaLinDueToR,normal);
//			//
//			//
//			//		float Jmod = -(e+1)*(vai).magnitude/scalar;
//			//		Vector3 J = normal*Jmod;
//			//			Vector3 vaf = vai - J*(1/ma);
//			//			grb.velocity = vaf;
//			//			Debug.Log ("initial is"+vai+"final is"+vaf);
//			//		grb.angularVelocity = wai - angularVelChangea;
//		}
        public void CollisionResponce(Rigidbody grb, Rigidbody trb, float e = 0.5F)
        {
            float mb = trb.mass;
            float ma = grb.mass;
            //manually set first
            Vector3 n = hitNormal;


            Vector3  vai = grb.velocity;
            Vector3  vbi = trb.velocity;
            Vector3  wai = grb.angularVelocity;
            Vector3  wbi = trb.angularVelocity;
            Vector3  ita = grb.inertiaTensor;
            Vector3  itb = trb.inertiaTensor;
            Matrix3f Qa  = Matrix3f.rotation(grb.inertiaTensorRotation);
            Matrix3f QaT = Qa.Transposed();
            Matrix3f Qb  = Matrix3f.rotation(trb.inertiaTensorRotation);
            Matrix3f QbT = Qb.Transposed();
            Matrix3f Aa  = new Matrix3f(ita);
            Matrix3f Ab  = new Matrix3f(itb);
            Matrix3f Ia  = Qa * Aa * QaT;
            Matrix3f Ib  = Qb * Ab * QbT;
            Vector3  ra  = grb.position - grb.worldCenterOfMass;
            Vector3  rb  = trb.position - trb.worldCenterOfMass;

            Matrix3f IaInverse         = Ia.inverse();
            Vector3  normal            = n.normalized;
            Vector3  angularVelChangea = normal;                 // start calculating the change in abgular rotation of a

            angularVelChangea = Vector3.Cross(angularVelChangea, ra);

            Vector3  vaLinDueToR       = Vector3.Cross(IaInverse * angularVelChangea, ra);         // calculate the linear velocity of collision point on a due to rotation of a
            float    scalar            = 1 / ma + Vector3.Dot(vaLinDueToR, normal);
            Matrix3f IbInverse         = Ib.inverse();
            Vector3  angularVelChangeb = normal;                // start calculating the change in abgular rotation of b

            angularVelChangeb = Vector3.Cross(angularVelChangeb, rb);

            Vector3 vbLinDueToR = Vector3.Cross(IbInverse * angularVelChangeb, rb);               // calculate the linear velocity of collision point on b due to rotation of b

            scalar += 1 / mb + Vector3.Dot(vbLinDueToR, normal);
            float   Jmod = (e + 1) * (vai - vbi).magnitude / scalar;
            Vector3 J    = normal * Jmod;
            Vector3 vaf  = vai - J * (1 / ma);

            grb.velocity = vaf;
            Debug.Log("initial is" + vai + "final is" + vaf);
            trb.velocity        = vbi - J * (1 / mb);
            grb.angularVelocity = wai - angularVelChangea;
            trb.angularVelocity = wbi - angularVelChangeb;
        }
Esempio n. 10
0
        public Matrix3f Transposed()
        {
            Matrix3f m = new Matrix3f();

            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 3; ++j)
                {
                    m.getElements()[GetIndex(j, i)] = this.getElements()[GetIndex(i, j)];
                }
            }

            return(m);
        }
Esempio n. 11
0
        public void CollisionResponce(Rigidbody grb, Rigidbody trb, Vector3 v, Vector3 hitNormal, float e = 0.3F)
        {
            //coefficient of restitution: ratio of speed after/before

            float   ma  = grb.mass;
            float   mb  = trb.mass;
            Vector3 n   = hitNormal.normalized;
            Vector3 vai = grb.velocity;
            Vector3 vbi = trb.velocity;

            Vector3 wai = grb.angularVelocity;
            Vector3 wbi = trb.angularVelocity;

            Vector3  ita = grb.inertiaTensor;
            Matrix3f Ia  = new Matrix3f(ita);
//			print (Aa+" ita:"+ita+" itra:"+itra+" Qa:"+Qa+" QaT:"+QaT);
//			Matrix3f Ia = Qa * Aa * QaT;
//			Ia.print();
            Matrix3f IaInverse = Ia.inverse();

//			print ("Pos:"+v+" CM:" + grb.centerOfMass);
//			//need to change, this is point of impact
            Vector3 ra = (v - grb.worldCenterOfMass);

            Vector3  itb       = trb.inertiaTensor;
            Matrix3f Ib        = new Matrix3f(itb);
            Matrix3f IbInverse = Ib.inverse();
            Vector3  rb        = impactpt - trb.worldCenterOfMass;

            Vector3 normal            = n.normalized;
            Vector3 angularVelChangea = Vector3.Cross(normal, ra);
            Vector3 vaLinDueToR       = Vector3.Cross(IaInverse * angularVelChangea, ra);         // calculate the linear velocity of collision point on a due to rotation of a
            Vector3 angularVelChangeb = Vector3.Cross(normal, rb);
            Vector3 vbLinDueToR       = Vector3.Cross(IbInverse * angularVelChangeb, rb);
//
            float scalar = 1 / ma + 1 / mb + Vector3.Dot(vaLinDueToR, normal) + Vector3.Dot(vbLinDueToR, normal);


            float   Jmod = -(e + 1) * (vai - vbi).magnitude / scalar;
            Vector3 J    = normal * Jmod;
            Vector3 vaf  = vai - J * (1 / ma);

            grb.velocity = vaf;
            trb.velocity = -vbi + J * (1 / mb);
            Vector3 deltawa = IaInverse * Vector3.Cross(J, ra);
            Vector3 deltawb = IbInverse * Vector3.Cross(J, rb);

            grb.angularVelocity = wai - deltawa;
            trb.angularVelocity = wbi - deltawb;
        }
Esempio n. 12
0
    // RIGIDBODY FUNCTIONS
    public void SetVariables()
    {
        position = ns_position;
        //m_linearAcceleration = ns_m_linearAcceleration;
        //position = transform.position;
        orientation       = ns_orientation;
        m_linearVelocity  = ns_m_linearVelocity;
        m_AngularVelocity = ns_m_AngularVelocity;
        Torque            = ns_Torque;
        AngularMomentum   = ns_AngularMomentum;

        //LinearMomentum = ns_LinearMomentum;
        m_AngularAccelration      = ns_AngularMomentum;
        WorldInertiaInverseTensor = ns_WorldInertiaInverseTensor;
    }
Esempio n. 13
0
        public static Matrix3f operator +(Matrix3f x, Matrix3f y)
        {
            Matrix3f product = new Matrix3f();             // zeroes

            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 3; ++j)
                {
                    product.m_elements[product.GetIndex(i, j)] = (x.getElement(i, j) + y.getElement(i, j));
                    //product.setElement(i, k, product.getElement(i, k) + x.getElement(i, j) * y.getElement(j, k));
                }
            }

            return(product);
        }
Esempio n. 14
0
        public void CollisionResponce(RigidBody grb, Rigidbody trb, Vector3 v, Vector3 hitNormal, float e = 0.5F)
        {
            //coefficient of restitution: ratio of speed after/before

            Vector3 targetNormal = hitNormal.normalized;
            Vector3 n            = hitNormal.normalized;
            float   ma           = grb.Mass;
            float   mb           = trb.mass;

            Vector3 vai = grb.m_linearVelocity;
            Vector3 vbi = trb.velocity;

            Vector3 wai = grb.m_AngularVelocity;
            Vector3 wbi = trb.angularVelocity;

            Vector3  ita       = grb.GetInertiaTensor();
            Matrix3f IaInverse = grb.WorldInertiaInverseTensor;
            Vector3  ra        = (v - grb.ns_position);

            Vector3  itb       = trb.inertiaTensor;
            Matrix3f Ib        = new Matrix3f(itb);
            Matrix3f IbInverse = Ib.inverse();
            Vector3  rb        = v - trb.worldCenterOfMass;

            Vector3 normal            = -n.normalized;
            Vector3 angularVelChangea = Vector3.Cross(normal, ra);                        // start calculating the change in angular rotation of a
            Vector3 vaLinDueToR       = Vector3.Cross(IaInverse * angularVelChangea, ra); // calculate the linear velocity of collision point on a due to rotation of a
            Vector3 angularVelChangeb = Vector3.Cross(normal, rb);
            Vector3 vbLinDueToR       = Vector3.Cross(IbInverse * angularVelChangeb, rb);
            // calculate the linear velocity of collision point on a due to rotation of a
            float scalar = 1 / ma + 1 / mb + Vector3.Dot(vaLinDueToR, normal) + Vector3.Dot(vbLinDueToR, normal);


            float   Jmod = -(e + 1) * (vai - vbi).magnitude / scalar;
            Vector3 J    = normal * Jmod;
            Vector3 vaf  = vai - J * grb.inv_mass;

            grb.m_linearVelocity = vaf;
            trb.velocity         = -vbi + J * (1 / mb);
            Vector3 deltawa = IaInverse * Vector3.Cross(J, ra);
            Vector3 deltawb = IbInverse * Vector3.Cross(J, rb);

            grb.m_AngularVelocity = wai - deltawa;
            trb.angularVelocity   = wbi - deltawb;
        }
Esempio n. 15
0
    // UNITY DEFINED FUNCTIONS
    void Start()
    {
        //initialise the initial conditions

        /*
         * InverseInertiaTensor
         *
         */
        ns_position    = transform.position;
        ns_orientation = orientation.QuaterniontoMatrix3(transform.rotation);
        float halfX = GetComponent <Renderer>().bounds.extents.x;
        float halfY = GetComponent <Renderer>().bounds.extents.y;
        float halfZ = GetComponent <Renderer>().bounds.extents.z;

        BodyInertiaInverseTensor.setMELEMENT(0, 0, 12f / (Mass * (halfY * halfY + halfZ * halfZ)));
        BodyInertiaInverseTensor.setMELEMENT(1, 1, 12f / (Mass * (halfX * halfX + halfZ * halfZ)));
        BodyInertiaInverseTensor.setMELEMENT(2, 2, 12f / (Mass * (halfX * halfX + halfY * halfY)));
    }
Esempio n. 16
0
        public static Matrix3f operator *(Matrix3f x, float y)
        {
            Matrix3f product = new Matrix3f();     // zeroes


            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 3; ++j)
                {
                    for (int k = 0; k < 3; ++k)
                    {
                        product.m_elements[product.GetIndex(i, k)] *= y;
                        //product.setElement(i, k, product.getElement(i, k) + x.getElement(i, j) * y.getElement(j, k));
                    }
                }
            }

            return(product);
        }
Esempio n. 17
0
    void Integrate()
    {
        for (int counter = 0; counter < NumberOfBodies; counter++)
        {
            //get the gameobject
            GameObject gameObject = (GameObject)allBodies[counter];
            //get the rigidbody  of the object
            RigidBody rigidBody = gameObject.GetComponent <RigidBody>();

            //linear
            //set position first
            if (rigidBody.position != rigidBody.transform.position)
            {
                rigidBody.position = rigidBody.transform.position;
            }
            rigidBody.ns_position = rigidBody.position + rigidBody.m_linearVelocity * Time.fixedDeltaTime;
            //set the velocity
            rigidBody.ns_m_linearVelocity = rigidBody.m_linearVelocity + rigidBody.inv_mass * rigidBody.forces * Time.fixedDeltaTime;

            //set the acceleration
            //rigidBody.ns_m_linearAcceleration += rigidBody.forces * rigidBody.inv_mass;



            //rotational

            GK.Matrix3f skewedAngurlar = new GK.Matrix3f();
            skewedAngurlar = skewedAngurlar.SkewSymmetry(rigidBody.m_AngularVelocity);

            rigidBody.ns_orientation = rigidBody.orientation + skewedAngurlar * Time.fixedDeltaTime * rigidBody.orientation;

            rigidBody.ns_AngularMomentum = rigidBody.AngularMomentum + Time.fixedDeltaTime * rigidBody.Torque;

            GK.Matrix3f transposedOrientation = new GK.Matrix3f();
            transposedOrientation = rigidBody.ns_orientation.Transposed();
            rigidBody.ns_WorldInertiaInverseTensor = (rigidBody.ns_orientation * rigidBody.BodyInertiaInverseTensor) * transposedOrientation;

            rigidBody.ns_orientation.OrthonormalizeOrientation();

            rigidBody.ns_m_AngularVelocity = rigidBody.ns_WorldInertiaInverseTensor * rigidBody.ns_AngularMomentum;
        }
    }
Esempio n. 18
0
        public Quaternion matrixs3fToQuaternion(Matrix3f m)
        {
            Quaternion q     = new Quaternion();
            float      trace = m.m_elements[GetIndex(0, 0)] + m.m_elements[GetIndex(1, 1)] + m.m_elements[GetIndex(2, 2)];

            if (trace > 0)
            {
                float s = 0.5f / Mathf.Sqrt(trace + 1.0f);
                q.w = 0.25f / s;
                q.x = (m.m_elements[GetIndex(2, 1)] - m.m_elements[GetIndex(1, 2)]) * s;
                q.y = (m.m_elements[GetIndex(0, 2)] - m.m_elements[GetIndex(2, 0)]) * s;
                q.z = (m.m_elements[GetIndex(1, 0)] - m.m_elements[GetIndex(0, 1)]) * s;
            }
            else
            {
                if (m.m_elements[GetIndex(0, 0)] > m.m_elements[GetIndex(1, 1)] && m.m_elements[GetIndex(0, 0)] > m.m_elements[GetIndex(2, 2)])
                {
                    float s = 2.0f * Mathf.Sqrt(1.0f + m.m_elements[GetIndex(0, 0)] - m.m_elements[GetIndex(1, 1)] - m.m_elements[GetIndex(2, 2)]);
                    q.w = (m.m_elements[GetIndex(2, 1)] - m.m_elements[GetIndex(1, 2)]) / s;
                    q.x = 0.25f * s;
                    q.y = (m.m_elements[GetIndex(0, 1)] + m.m_elements[GetIndex(1, 0)]) / s;
                    q.z = (m.m_elements[GetIndex(0, 2)] + m.m_elements[GetIndex(2, 0)]) / s;
                }
                else if (m.m_elements[GetIndex(1, 1)] > m.m_elements[GetIndex(2, 2)])
                {
                    float s = 2.0f * Mathf.Sqrt(1.0f + m.m_elements[GetIndex(1, 1)] - m.m_elements[GetIndex(0, 0)] - m.m_elements[GetIndex(2, 2)]);
                    q.w = (m.m_elements[GetIndex(0, 2)] - m.m_elements[GetIndex(2, 0)]) / s;
                    q.x = (m.m_elements[GetIndex(0, 1)] + m.m_elements[GetIndex(1, 0)]) / s;
                    q.y = 0.25f * s;
                    q.z = (m.m_elements[GetIndex(1, 2)] + m.m_elements[GetIndex(2, 1)]) / s;
                }
                else
                {
                    float s = 2.0f * Mathf.Sqrt(1.0f + m.m_elements[GetIndex(2, 2)] - m.m_elements[GetIndex(0, 0)] - m.m_elements[GetIndex(1, 1)]);
                    q.w = (m.m_elements[GetIndex(1, 0)] - m.m_elements[GetIndex(0, 1)]) / s;
                    q.x = (m.m_elements[GetIndex(0, 2)] + m.m_elements[GetIndex(2, 0)]) / s;
                    q.y = (m.m_elements[GetIndex(1, 2)] + m.m_elements[GetIndex(2, 1)]) / s;
                    q.z = 0.25f * s;
                }
            }
            return(q);
        }
Esempio n. 19
0
    public void SetRotation(GK.Matrix3f Orientation)
    {
        float sy = Mathf.Sqrt(Orientation.getElement(0, 0) * Orientation.getElement(0, 0) + Orientation.getElement(1, 0) * Orientation.getElement(1, 0));

        bool singular = sy < 1e-6;         // If

        float x, y, z;

        if (!singular)
        {
            x = Mathf.Atan2(Orientation.getElement(2, 1), Orientation.getElement(2, 2));
            y = Mathf.Atan2(-Orientation.getElement(2, 0), sy);
            z = Mathf.Atan2(Orientation.getElement(1, 0), Orientation.getElement(0, 0));
        }
        else
        {
            x = Mathf.Atan2(-Orientation.getElement(1, 2), Orientation.getElement(1, 1));
            y = Mathf.Atan2(-Orientation.getElement(2, 0), sy);
            z = 0;
        }
        Debug.Log("euler" + new Vector3(x, y, z));
        transform.Rotate(new Vector3(x, y, z));
    }
Esempio n. 20
0
        public void CollisionResponce(RigidBody grb, Rigidbody trb, Vector3 v, Vector3 hitNormal, float e = 0.5F)
        {
            //coefficient of restitution: ratio of speed after/before

            Vector3 targetNormal = hitNormal.normalized;
            Vector3 n            = hitNormal.normalized;
            //Debug.Log("targetnormal");
            //Debug.Log(targetNormal);
            //Vector3 ra = v - grb.position;
            //Debug.Log("ra");
            //Debug.Log(ra);
            //Vector3 Velocity = grb.m_linearVelocity + Vector3.Cross(grb.m_AngularVelocity, ra);
            //Debug.Log("velocity");
            //Debug.Log(Velocity);
            //float ImpulseNumerator = -(1.0f + e) * Vector3.Dot(Velocity, targetNormal);
            //Debug.Log("ImpulseNumerator");
            //Debug.Log(ImpulseNumerator);
            //float ImpulseDenominator = grb.inv_mass + Vector3.Dot(Vector3.Cross(grb.WorldInertiaInverseTensor * Vector3.Cross(ra, targetNormal), ra), hitNormal);
            //Debug.Log("ImpulseDenominator");
            //Debug.Log(ImpulseDenominator);
            //Vector3 J = (ImpulseNumerator * 1.0f / ImpulseDenominator * 1.0f) * targetNormal;
            //Debug.Log("J");
            //Debug.Log(J);
            //grb.m_linearVelocity += grb.inv_mass * J;
            //grb.AngularMomentum += Vector3.Cross(ra, J);

            //grb.m_AngularVelocity = grb.WorldInertiaInverseTensor * grb.AngularMomentum;

            float ma = grb.Mass;
            float mb = trb.mass;

            Vector3 vai = grb.m_linearVelocity;
            Vector3 vbi = trb.velocity;

            Vector3 wai = grb.m_AngularVelocity;
            Vector3 wbi = trb.angularVelocity;

            Vector3 ita = grb.GetInertiaTensor();


            Matrix3f IaInverse = grb.WorldInertiaInverseTensor;
            //need to change, this is point of impact
            Vector3 ra = (v - grb.ns_position);

            Vector3  itb       = trb.inertiaTensor;
            Matrix3f Ib        = new Matrix3f(itb);
            Matrix3f IbInverse = Ib.inverse();
            Vector3  rb        = v - trb.worldCenterOfMass;

            Vector3 normal            = -n.normalized;
            Vector3 angularVelChangea = Vector3.Cross(normal, ra);                        // start calculating the change in angular rotation of a
            Vector3 vaLinDueToR       = Vector3.Cross(IaInverse * angularVelChangea, ra); // calculate the linear velocity of collision point on a due to rotation of a
            Vector3 angularVelChangeb = Vector3.Cross(normal, rb);
            Vector3 vbLinDueToR       = Vector3.Cross(IbInverse * angularVelChangeb, rb);
            // calculate the linear velocity of collision point on a due to rotation of a
            float scalar = 1 / ma + 1 / mb + Vector3.Dot(vaLinDueToR, normal) + Vector3.Dot(vbLinDueToR, normal);


            float   Jmod = -(e + 1) * (vai).magnitude / scalar;
            Vector3 J    = normal * Jmod;
            Vector3 vaf  = vai - J * grb.inv_mass;

            grb.m_linearVelocity = vaf;
            trb.velocity         = -vbi + J * (1 / mb);
            Vector3 deltawa = IaInverse * Vector3.Cross(J, ra);
            Vector3 deltawb = IbInverse * Vector3.Cross(J, rb);

            grb.m_AngularVelocity = wai - deltawa;
            trb.angularVelocity   = wbi - deltawb;

            //float ma = grb.Mass;
            //float mb = trb.Mass;

            //Vector3 vai = grb.m_linearVelocity;
            //Vector3 vbi = trb.m_linearVelocity;

            //Vector3 wai = grb.m_AngularVelocity;
            //Vector3 wbi = trb.m_AngularVelocity;

            //Vector3 ita = grb.GetInertiaTensor();


            //Matrix3f IaInverse = grb.WorldInertiaInverseTensor;
            ////need to change, this is point of impact
            //Vector3 ra = (v - grb.ns_position);

            ////Vector3 itb = trb.World;
            ////Matrix3f Ib = new Matrix3f(itb);
            //Matrix3f IbInverse = trb.WorldInertiaInverseTensor;
            //Vector3 rb = v - trb.ns_position;

            //Vector3 normal = n.normalized;
            //Vector3 angularVelChangea = Vector3.Cross(normal, ra); // start calculating the change in angular rotation of a
            //Vector3 vaLinDueToR = Vector3.Cross(IaInverse * angularVelChangea, ra);  // calculate the linear velocity of collision point on a due to rotation of a
            //Vector3 angularVelChangeb = Vector3.Cross(normal, rb);
            //Vector3 vbLinDueToR = Vector3.Cross(IbInverse * angularVelChangeb, rb);
            //// calculate the linear velocity of collision point on a due to rotation of a
            //float scalar = 1 / ma + 1 / mb + Vector3.Dot(vaLinDueToR, normal) + Vector3.Dot(vbLinDueToR, normal);


            //float Jmod = -(e + 1) * (vai).magnitude / scalar;
            //Vector3 J = normal * Jmod;
            //Vector3 vaf = vai - J * grb.inv_mass;
            //grb.m_linearVelocity = vaf;
            //trb.m_linearVelocity = -vbi + J * (1 / mb);
            //Vector3 deltawa = IaInverse * Vector3.Cross(J, ra);
            //Vector3 deltawb = IbInverse * Vector3.Cross(J, rb);
            //grb.m_AngularVelocity = wai - deltawa;
            //trb.m_AngularVelocity = wbi - deltawb;
        }