private void Awake()
{
_renderer = GetComponent <SpriteRenderer>();
_jRigidbody = GetComponent <JRigidbody>();
_jRigidbody.onCollisionEnter += CollisionEnter;
_jRigidbody.onCollisionExit += CollisionExit;
}
private bool GetBodyCollisions(JRigidbody bodyA, JRigidbody bodyB, ref List <JCollision> collisions)
{
if (CheckIfBoundsCollide(bodyA.GetBounds(), bodyB.GetBounds()))
{
return(GetBodyCollisionsWithColliders(bodyA, bodyB, ref collisions));
}
return(false);
}
private bool GetBodyCollisionsWithColliders(JRigidbody bodyA, JRigidbody bodyB, ref List <JCollision> collisions)
{
JCollider[] collidersA = bodyA.GetColliders();
JCollider[] collidersB = bodyB.GetColliders();
for (int i = 0; i < collidersA.Length; i++)
{
for (int j = 0; j < collidersB.Length; j++)
{
JCollision collision;
if (JCollisionSolver.SolveCollision(collidersA[i], collidersB[j], out collision))
{
collisions.Add(collision);
}
}
}
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);
}
public static void DeregisterBody(JRigidbody body)
{
_bodies.Remove(body);
}
public static void RegisterBody(JRigidbody body)
{
_bodies.Add(body);
Debug.Log("Registered " + body.name);
}
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);
}
}
}
private void Start()
{
body = GetComponent <JRigidbody>();
}
