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