// 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; }
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; }
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; }
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; }