public void CalculateImpulse(Collision.CollisionData manifold, float deltaTime)
{
// reference on how to calculate and use impulse force:
// https://gamedevelopment.tutsplus.com/tutorials/how-to-create-a-custom-2d-physics-engine-the-basics-and-impulse-resolution--gamedev-6331
if (manifold.bodyA == null || manifold.bodyB == null)
{
Debug.LogWarning("Missing Rigidbody");
return;
}
// Calculate relative velocity
Vector3 relativeVelocity = manifold.bodyB.velocity - manifold.bodyA.velocity;
// Calculate relative velocity in terms of the normal direction
Vector3 collisionNormal = manifold.normal;
float velocityAlongNormal = Vector3.Dot(relativeVelocity, collisionNormal);
// Return if moving away from eachother as will be seperated anyways
if (velocityAlongNormal > 0.0f)
{
return;
}
// Calculate restitution
float restituion = Mathf.Min(manifold.bodyA._restitution, manifold.bodyB._restitution);
// One static, one dynamic
if ((manifold.bodyA.velocity == Vector3.zero && manifold.bodyB.velocity != Vector3.zero) ||
(manifold.bodyB.velocity == Vector3.zero && manifold.bodyA.velocity != Vector3.zero))
{
// Get moving object
NewRigidBody movingBody;
if (manifold.bodyA.velocity != Vector3.zero)
{
movingBody = manifold.bodyA;
}
else
{
movingBody = manifold.bodyB;
}
restituion = movingBody._restitution;
Vector3 newVelocity = movingBody.velocity * restituion;
if (Mathf.Abs(collisionNormal.x) > Mathf.Abs(collisionNormal.y) && Mathf.Abs(collisionNormal.x) > Mathf.Abs(collisionNormal.z))
{
newVelocity.x = -newVelocity.x;
}
else if (Mathf.Abs(collisionNormal.y) > Mathf.Abs(collisionNormal.x) && Mathf.Abs(collisionNormal.y) > Mathf.Abs(collisionNormal.z))
{
newVelocity.y = -newVelocity.y;
}
else if (Mathf.Abs(collisionNormal.z) > Mathf.Abs(collisionNormal.x) && Mathf.Abs(collisionNormal.z) > Mathf.Abs(collisionNormal.y))
{
newVelocity.z = -newVelocity.z;
}
if (movingBody._isKinematic == false)
{
movingBody.velocity = newVelocity;
}
// Friction
{
float j = -(1.0f + movingBody._restitution) * velocityAlongNormal;
j /= manifold.bodyA._inverseMass + manifold.bodyB._inverseMass;
ApplyFriction(manifold, j, deltaTime);
}
if (manifold.contacts != null)
{
if (manifold.contacts.Count > 0)
{
float impulseRot = 0.0f;
impulseRot += Vector3.Cross((manifold.contacts [0] - movingBody.transform.position), collisionNormal).sqrMagnitude *movingBody._inverseInertia;
float jRot = -(1.0f + movingBody._restitution) * velocityAlongNormal;
jRot /= manifold.bodyA._inverseMass + manifold.bodyB._inverseMass + impulseRot;
movingBody.angularVelocity -= manifold.bodyA._inverseInertia * Vector3.Cross(manifold.contacts [0], jRot * -collisionNormal) * deltaTime;
}
}
}
// Two Dyanmic objects
else
{
// Calculate impulse scalar
float j = -(1.0f + restituion) * velocityAlongNormal;
if (manifold.contacts == null || manifold.contacts.Count == 0)
{
j /= manifold.bodyA._inverseMass + manifold.bodyB._inverseMass;
}
else
{
// with rotation
float rA = Vector3.Cross((manifold.contacts [0] - manifold.bodyAobj.transform.position), -collisionNormal).sqrMagnitude;
float rB = Vector3.Cross((manifold.contacts [0] - manifold.bodyBobj.transform.position), -collisionNormal).sqrMagnitude;
float impulseRotA = rA * manifold.bodyA._inverseInertia;
float impulseRotB = rB * manifold.bodyB._inverseInertia;
j /= manifold.bodyA._inverseMass + manifold.bodyB._inverseMass + impulseRotA + impulseRotB;
// Apply
if (manifold.bodyA._isKinematic == false)
{
manifold.bodyA.angularVelocity -= manifold.bodyA._inverseInertia * Vector3.Cross(manifold.contacts [0], j * -collisionNormal) * deltaTime;
}
if (manifold.bodyB._isKinematic == false)
{
manifold.bodyB.angularVelocity += manifold.bodyB._inverseInertia * Vector3.Cross(manifold.contacts[0], j * -collisionNormal) * deltaTime;
}
}
// Apply impulse
Vector3 impulse = j * collisionNormal;
if (manifold.bodyA._isKinematic == false)
{
manifold.bodyA.velocity -= manifold.bodyA._inverseMass * impulse;
}
if (manifold.bodyB._isKinematic == false)
{
manifold.bodyB.velocity += manifold.bodyB._inverseMass * impulse;
}
// Friction
ApplyFriction(manifold, j, deltaTime);
}
PositionCorrection(manifold.bodyA, manifold.bodyB, manifold.penetration, collisionNormal);
}
private void ApplyFriction(Collision.CollisionData manifold, float j, float deltaTime)
{
// reference on how to get friction impulse and tangent:
// https://gamedevelopment.tutsplus.com/tutorials/how-to-create-a-custom-2d-physics-engine-friction-scene-and-jump-table--gamedev-7756
Vector3 relativeVelocity = manifold.bodyB.velocity - manifold.bodyA.velocity;
Vector3 tangent = Vector3.Normalize(relativeVelocity - Vector3.Dot(relativeVelocity, manifold.normal) * manifold.normal);
float jFriction = -Vector3.Dot(relativeVelocity, tangent);
if (manifold.contacts == null || manifold.contacts.Count == 0)
{
jFriction = jFriction / (manifold.bodyA._inverseMass + manifold.bodyB._inverseMass);
}
// with rotation
else
{
float rA = Vector3.Cross((manifold.contacts [0] - manifold.bodyAobj.transform.position), -manifold.normal).sqrMagnitude;
float rB = Vector3.Cross((manifold.contacts [0] - manifold.bodyBobj.transform.position), -manifold.normal).sqrMagnitude;
float impulseRotA = rA * manifold.bodyA._inverseInertia;
float impulseRotB = rB * manifold.bodyB._inverseInertia;
jFriction = jFriction / (manifold.bodyA._inverseMass + manifold.bodyB._inverseMass + impulseRotA + impulseRotB);
// Apply
if (manifold.bodyA._isKinematic == false)
{
manifold.bodyA.angularVelocity -= manifold.bodyA._inverseInertia * Vector3.Cross(manifold.contacts [0], jFriction * -manifold.normal) * deltaTime;
}
if (manifold.bodyB._isKinematic == false)
{
manifold.bodyB.angularVelocity += manifold.bodyB._inverseInertia * Vector3.Cross(manifold.contacts [0], jFriction * -manifold.normal) * deltaTime;
}
}
float approx = Mathf.Sqrt((manifold.bodyA.staticFriction * manifold.bodyA.staticFriction)
+ (manifold.bodyB.staticFriction * manifold.bodyB.staticFriction));
Vector3 frictionImpulse;
if (Mathf.Abs(jFriction) < j * approx)
{
frictionImpulse = jFriction * tangent;
}
else
{
float dynamicFriction = Mathf.Sqrt((manifold.bodyA._dynamicFriction * manifold.bodyA._dynamicFriction)
+ (manifold.bodyB._dynamicFriction * manifold.bodyB._dynamicFriction));
frictionImpulse = -j * tangent * dynamicFriction;
}
// Apply friction impulse
if (manifold.bodyA._isKinematic == false)
{
manifold.bodyA.velocity -= manifold.bodyA._inverseMass * frictionImpulse;
}
if (manifold.bodyB._isKinematic == false)
{
manifold.bodyB.velocity += manifold.bodyB._inverseMass * frictionImpulse;
}
}
bool IsCollision(float deltaTime)
{
if (this._isKinematic == true)
{
return(false);
}
bool wasCollision = false;
if (this.GetComponent <NewSphereCollider>() != null)
{
NewBoxCollider[] boxes = GameObject.FindObjectsOfType <NewBoxCollider>();
foreach (NewBoxCollider box in boxes)
{
Collision.CollisionData mani = Collision.instance.BoxToSphereManifold(box, this.GetComponent <NewSphereCollider> ());
if (mani.wasCollision)
{
CalculateImpulse(mani, deltaTime);
wasCollision = true;
}
}
NewSphereCollider[] spheres = GameObject.FindObjectsOfType <NewSphereCollider>();
foreach (NewSphereCollider sphere in spheres)
{
if (sphere.gameObject.GetInstanceID() == this.gameObject.GetInstanceID())
{
continue;
}
Collision.CollisionData mani = Collision.instance.SphereToSphereManifold(this.GetComponent <NewSphereCollider> (), sphere);
if (mani.wasCollision)
{
CalculateImpulse(mani, deltaTime);
wasCollision = true;
}
}
}
else if (this.GetComponent <NewBoxCollider>() != null)
{
NewBoxCollider[] boxes = GameObject.FindObjectsOfType <NewBoxCollider>();
foreach (NewBoxCollider box in boxes)
{
if (box.gameObject.GetInstanceID() == this.gameObject.GetInstanceID())
{
continue;
}
Vector3 bOrientation = box.GetComponent <NewRigidBody>().orientation;
Collision.CollisionData mani = new Collision.CollisionData();
// if axis aligned
if ((this.orientation.x == 0 || this.orientation.x == 90 || this.orientation.x == 180 || this.orientation.x == 270) &&
(this.orientation.y == 0 || this.orientation.y == 90 || this.orientation.y == 180 || this.orientation.y == 270) &&
(this.orientation.z == 0 || this.orientation.z == 90 || this.orientation.z == 180 || this.orientation.z == 270)
&&
(bOrientation.x == 0 || bOrientation.x == 90 || bOrientation.x == 180 || bOrientation.x == 270) &&
(bOrientation.y == 0 || bOrientation.y == 90 || bOrientation.y == 180 || bOrientation.y == 270) &&
(bOrientation.z == 0 || bOrientation.z == 90 || bOrientation.z == 180 || bOrientation.z == 270))
{
mani = Collision.instance.BoxToBoxManifold(this.GetComponent <NewBoxCollider>(), box);
}
// otherwise SAT
else
{
mani = Collision.instance.BoxToBoxSAT(this.GetComponent <NewBoxCollider>(), box);
}
if (mani.wasCollision)
{
CalculateImpulse(mani, deltaTime);
wasCollision = true;
}
}
NewSphereCollider[] spheres = GameObject.FindObjectsOfType <NewSphereCollider>();
foreach (NewSphereCollider sphere in spheres)
{
Collision.CollisionData mani = Collision.instance.BoxToSphereManifold(this.GetComponent <NewBoxCollider> (), sphere);
if (mani.wasCollision)
{
CalculateImpulse(mani, deltaTime);
wasCollision = true;
}
}
}
return(wasCollision);
}
