protected override bool CheckCollision(JCollider colliderA, JCollider colliderB, out JCollision collision)
{
collision = null;
if (colliderA is T1 && colliderB is T2)
{
collision = new JCollision(colliderA, colliderB);
return(CheckCollision(colliderA as T1, colliderB as T2, out collision));
}
else if (colliderB is T1 && colliderA is T2)
{
collision = new JCollision(colliderB, colliderA);
return(CheckCollision(colliderB as T1, colliderA as T2, out collision));
}
return(false);
}
private void OnDrawGizmos()
{
Gizmos.color = Color.yellow;
if (wasHit)
{
Gizmos.DrawSphere(testPoint, 0.1f);
}
Gizmos.color = Color.blue;
for (int i = 0; i < _frameCollisions.Count; i++)
{
JCollision collision = _frameCollisions[i];
for (int j = 0; j < collision.collisionPoints.Count; j++)
{
Gizmos.DrawSphere(collision.collisionPoints[j], 0.1f);
}
}
}
public override bool Equals(object obj)
{
if (obj == null)
{
return(false);
}
JCollision collision = (JCollision)obj;
if (collision != null)
{
bool equal =
(colliderA == collision.colliderA && colliderB == collision.colliderB) ||
(colliderA == collision.colliderB && colliderB == collision.colliderA);
return(equal);
}
return(false);
}
private void CorrectPositions(JCollision collision)
{
const float slop = 0.01f;
const float percent = 0.6f;
JRigidbody bodyA = collision.colliderA.owningBody;
JRigidbody bodyB = collision.colliderB.owningBody;
Vector3 correction = (Mathf.Max(collision.collisionDepth - slop, 0.0f) / (bodyA.Mass + bodyB.Mass)) * percent * collision.collisionNormal;
if (!bodyA._isKinematic)
{
bodyA.transform.position -= bodyA.Mass * correction;
}
if (!bodyB._isKinematic)
{
bodyB.transform.position += bodyB.Mass * correction;
}
//bodyA.ApplyForce(bodyA.Mass * correction * 200);
//bodyB.ApplyForce(-bodyB.Mass * correction * 200);
}
protected override bool CheckCollision(JSphereCollider colliderA, JPlaneCollider colliderB, out JCollision collision)
{
Vector3 closestPointA = colliderB.GetClosestPoint(colliderA.transform.position);
Vector3 closestPointB = colliderA.GetClosestPoint(closestPointA, true);
Debug.DrawLine(closestPointA, closestPointB);
if (colliderB.IsPointInside(closestPointB))
{
float collisionDepth = Vector3.Distance(closestPointA, closestPointB);
Vector3 collisionNormal = (closestPointA - closestPointB).normalized;
List <Vector3> contacts = new List <Vector3>()
{
closestPointA + (closestPointB * 0.5f)
};
collision = new JCollision(contacts, collisionNormal, collisionDepth, colliderA, colliderB);
return(true);
}
collision = null;
return(false);
}
protected override bool CheckCollision(JCuboidCollider colliderA, JCuboidCollider colliderB, out JCollision collision)
{
collision = new JCollision();
Vector3[] axesOfSeperation = GetAxesOfSeperation(colliderA, colliderB);
Vector3 normal = Vector3.zero;
bool shouldFlip = false;
collision.collisionDepth = float.PositiveInfinity;
collision.colliderA = colliderA;
collision.colliderB = colliderB;
Vector3[] colliderAVerts = colliderA.GetVertices();
Vector3[] colliderBVerts = colliderB.GetVertices();
for (int i = 0; i < axesOfSeperation.Length; i++)
{
if (Vector3.SqrMagnitude(axesOfSeperation[i]) < 0.001f)// If out axis has a length of 0, some false positives can happen, soooooo lets just not do that
{
continue;
}
float depth = FindPenetrationDepth(colliderAVerts, colliderBVerts, axesOfSeperation[i], out shouldFlip);
if (depth <= 0)
{
return(false);
}
else if (depth < collision.collisionDepth)
{
collision.collisionDepth = depth;
normal = shouldFlip ? axesOfSeperation[i] * -1.0f : axesOfSeperation[i];
}
}
if (normal == Vector3.zero)
{
return(false);
}
Vector3 sharedPlaneAxis = normal.normalized;
List <Vector3> contacts = new List <Vector3>();
contacts.AddRange(CuboidIntersectionPoints(colliderB, colliderA));
contacts.AddRange(CuboidIntersectionPoints(colliderA, colliderB));
JSegment meshSegment = JMeshCollider.GetMeshSegmentOnAxis(colliderAVerts, sharedPlaneAxis);
float distance = (meshSegment.Length * 0.5f) - (collision.collisionDepth * 0.5f);
Vector3 pointOnPlane = colliderA.transform.position + sharedPlaneAxis * distance;
collision.collisionNormal = sharedPlaneAxis;
collision.valid = contacts.Count > 0;
for (int i = 0; i < contacts.Count; i++)
{
Vector3 contact = contacts[i];
contact = contact + (sharedPlaneAxis * Vector3.Dot(sharedPlaneAxis, pointOnPlane - contact));
bool unique = true;
for (int j = 0; j < collision.collisionPoints.Count; j++)
{
if (Vector3.Distance(contact, collision.collisionPoints[j]) < 0.2f)
{
unique = false;
}
}
if (unique)
{
collision.collisionPoints.Add(contact);
}
}
return(collision.valid);
}
private void SolveFrameCollisions()
{
for (int i = 0; i < _frameCollisions.Count; i++)
{
JCollision collision = _frameCollisions[i];
JRigidbody bodyA = collision.colliderA.owningBody;
JRigidbody bodyB = collision.colliderB.owningBody;
if (bodyA == null || bodyB == null)
{
continue;
}
Vector3 normal = collision.collisionNormal.normalized;
Vector3 velocityA = bodyA.Velocity;
Vector3 velocityB = bodyB.Velocity;
Matrix4x4 invTensorA = bodyA._colliders[0].GetInverseTensor(bodyA.Mass);
Matrix4x4 invTensorB = bodyB._colliders[0].GetInverseTensor(bodyB.Mass);
Debug.DrawLine(bodyA.transform.position, collision.collisionPoints[0], Color.red);
for (int c = 0; c < collision.collisionPoints.Count; c++)
{
Vector3 relativeContactPointA = collision.collisionPoints[c] - bodyA.transform.position;
Vector3 relativeContactPointB = collision.collisionPoints[c] - bodyB.transform.position;
Vector3 contactVelocity =
((velocityB + Vector3.Cross(bodyB.AngularVelocity, relativeContactPointB)) -
(velocityA + Vector3.Cross(bodyA.AngularVelocity, relativeContactPointA)));
float reactionForceMagnitude = Vector3.Dot(contactVelocity, normal);
if (reactionForceMagnitude > 0)
{
continue;
}
float numerator = (-(1.0f + epsilon) * reactionForceMagnitude);
float d1 = (bodyA.GetInvMass() + bodyB.GetInvMass());
Vector3 d2 = Vector3.Cross(invTensorA * Vector3.Cross(relativeContactPointA, normal), relativeContactPointA);
Vector3 d3 = Vector3.Cross(invTensorB * Vector3.Cross(relativeContactPointB, normal), relativeContactPointB);
float denominator = d1 + Vector3.Dot(normal, d2 + d3);
float j = (denominator == 0) ? 0 : (numerator / denominator);
j /= (float)collision.collisionPoints.Count;
Vector3 impulseReactionForce = (j * normal);
bodyA.SetVelocity(velocityA - impulseReactionForce * bodyA.GetInvMass());
bodyB.SetVelocity(velocityB + impulseReactionForce * bodyB.GetInvMass());
bodyA.AngularVelocity = bodyA.AngularVelocity - (Vector3)(invTensorA * Vector3.Cross(relativeContactPointA, impulseReactionForce));
bodyB.AngularVelocity = bodyB.AngularVelocity + (Vector3)(invTensorB * Vector3.Cross(relativeContactPointB, impulseReactionForce));
//Vector3 tangent = (contactVelocity - (Vector3.Dot(contactVelocity, normal) * normal)).normalized;
//float staticFrictionAverage = PythSolver(bodyA.StaticFriction, bodyB.StaticFriction);
//float dynamicFrictionAverage = PythSolver(bodyA.DynamicFriction, bodyB.DynamicFriction);
//numerator = -Vector3.Dot(contactVelocity, tangent);
//d1 = (bodyA.GetInvMass() + bodyB.GetInvMass());
//d2 = Vector3.Cross(invTensorA * Vector3.Cross(relativeContactPointA, tangent), relativeContactPointA);
//d3 = Vector3.Cross(invTensorB * Vector3.Cross(relativeContactPointB, tangent), relativeContactPointB);
//denominator = d1 + Vector3.Dot(tangent, d2 + d3);
//if (denominator == 0)
//{
// continue;
//}
//float jt = (numerator / denominator) / (float)collision.collisionPoints.Count;
//if (jt <= 0)
//{
// continue;
//}
//Debug.Log("test");
//if (jt > j * dynamicFrictionAverage)
//{
// jt = j * dynamicFrictionAverage;
//}
//else if (jt < -j * dynamicFrictionAverage)
//{
// jt = -j * dynamicFrictionAverage;
//}
//Vector3 tangentImpulse = tangent * jt;
//bodyA.SetVelocity(bodyA.Velocity - (tangentImpulse * bodyA.GetInvMass()));
//bodyB.SetVelocity(bodyB.Velocity + (tangentImpulse * bodyB.GetInvMass()));
//bodyA.AngularVelocity = bodyA.AngularVelocity - (Vector3)(invTensorA * Vector3.Cross(relativeContactPointA, impulseReactionForce));
//bodyB.AngularVelocity = bodyB.AngularVelocity + (Vector3)(invTensorB * Vector3.Cross(relativeContactPointB, impulseReactionForce));
}
if (Mathf.Abs(bodyA.Velocity.magnitude) < 0.1f)
{
bodyA.SetVelocity(Vector3.zero);
}
if (Mathf.Abs(bodyB.Velocity.magnitude) < 0.1f)
{
bodyB.SetVelocity(Vector3.zero);
}
if (Mathf.Abs(bodyA.AngularVelocity.magnitude) < 0.1f)
{
bodyA.AngularVelocity = (Vector3.zero);
}
if (Mathf.Abs(bodyB.AngularVelocity.magnitude) < 0.1f)
{
bodyB.AngularVelocity = (Vector3.zero);
}
}
}
protected abstract bool CheckCollision(T1 colliderA, T2 colliderB, out JCollision collision);