public void Impulse_Correction_for_one(My_Rigid_Body A)
{
float Percent = 0.3f;
float Threshhold = 0.03f;
Vector3 correction = Mathf.Max(penetration - Threshhold, 0.0f) / (1.0f / A.mass) * Percent * normal_point_of_contact;
A.Correction_Position += (1.0f / A.mass) * correction;
}
public void Impulse_Correction_for_Two(My_Rigid_Body A, My_Rigid_Body B)
{
float Percent = 0.1f;
float Threshhold = 0.03f;
Vector3 correction = Mathf.Max(penetration - Threshhold, 0.0f) / (1.0f / A.mass + 1.0f / B.mass) * Percent * normal_point_of_contact;
if (A.mass != 0)
{
A.Correction_Position -= 1.0f / A.mass * correction;
}
if (B.mass != 0)
{
B.Correction_Position += 1.0f / B.mass * correction;
}
}
public void Friction(My_Rigid_Body A, My_Rigid_Body B, float j)
{
float A_coefficient_static_friction = A.coeficient_static_friction;
float B_coefficient_static_friction = B.coeficient_static_friction;
float A_coefficient_dynamic_friction = A.coeficient_dynamic_friction;
float B_coefficient_dynamic_friction = B.coeficient_dynamic_friction;
Vector3 RV = B.Velocity - A.Velocity;
// Tangent vector
Vector3 tangent = RV - Vector3.Dot(RV, normal_point_of_contact) * normal_point_of_contact;
tangent.Normalize();
// Solve for magnitude to apply along the friction vector
float jt = -Vector3.Dot(RV, tangent);
jt = jt / ((1.0f / A.mass) + (1.0f / B.mass));
float mu = Mathf.Max(A_coefficient_static_friction, B_coefficient_static_friction);
// Clamp magnitude of friction and create impulse vector
Vector3 frictionImpulse;
if (Mathf.Abs(jt) < j * mu)
{
frictionImpulse = jt * tangent;
}
else
{
float dynamicFriction = Mathf.Max(A_coefficient_dynamic_friction, B_coefficient_dynamic_friction);
frictionImpulse = -j * tangent * dynamicFriction;
}
A.Velocity -= (1.0f / A.mass) * frictionImpulse;
B.Velocity += (1.0f / B.mass) * frictionImpulse;
}
private void Update_Velocity(My_Rigid_Body rigidbody)
{
rigidbody.Force = new Vector3(0.0f, rigidbody.gravity, 0.0f);
rigidbody.Velocity += rigidbody.Force * Time.fixedDeltaTime;
}
private void Semi_Implicit_Euler(My_Rigid_Body rigidbody)
{
rigidbody.transform.position = rigidbody.Correction_Position + (rigidbody.Velocity * Time.fixedDeltaTime);
rigidbody.Correction_Position = rigidbody.transform.position;
}
private void Reset_Force(My_Rigid_Body rigidbody)
{
rigidbody.Force = Vector3.zero;
}
public void Impulse_for_Two(My_Rigid_Body A, My_Rigid_Body B)
{
//Relative Velocity
Vector3 RV = B.Velocity - A.Velocity;
float VelocityAlongNormal = Vector3.Dot(RV, normal_point_of_contact);
if (VelocityAlongNormal > 0)
{
return;
}
// Choose lowest restitution
float e = Mathf.Min(A.coeficient_restitution, B.coeficient_restitution);
// Calculate Impulse Scalar
float J = -(1.0f + e) * VelocityAlongNormal;
J /= (1.0f / A.mass) + (1.0f / B.mass);
// Apply Impulse
Vector3 Impulse = J * normal_point_of_contact;
Vector3 New_Force_A = (1.0f / A.mass) * Impulse;
Vector3 New_Force_B = (1.0f / B.mass) * Impulse;
float sleep_threshold = 0.05f;
if (A.asleep_ == true)
{
if (A.mass != 0)
{
if ((New_Force_A.x > sleep_threshold || New_Force_A.y > sleep_threshold || New_Force_A.z > sleep_threshold))
{
A.asleep_ = false;
A.Velocity -= (1.0f / A.mass) * Impulse;
A.gameObject.GetComponent <Renderer>().material.color = Color.red;
}
}
}
else
{
if (A.mass != 0)
{
A.Velocity -= (1.0f / A.mass) * Impulse;
}
}
if (B.asleep_ == true)
{
if (B.mass != 0)
{
if ((New_Force_B.x > sleep_threshold || New_Force_B.y > sleep_threshold || New_Force_B.z > sleep_threshold))
{
B.asleep_ = false;
B.Velocity += (1.0f / B.mass) * Impulse;
B.gameObject.GetComponent <Renderer>().material.color = Color.red;
}
}
}
else
{
if (B.mass != 0)
{
B.Velocity += (1.0f / B.mass) * Impulse;
}
}
Impulse_Correction_for_Two(A, B);
if (A.gameObject.tag == "Plane" || A.gameObject.tag == "Wall")
{
Plane_Friction(B);
}
else if (B.gameObject.tag == "Plane" || B.gameObject.tag == "Wall")
{
Plane_Friction(A);
}
else
{
Friction(A, B, J);
}
if (A.gameObject.tag == "Plane")
{
B.midair = false;
}
else if (B.gameObject.tag == "Plane")
{
A.midair = false;
}
}
public void Plane_Friction(My_Rigid_Body A)
{
A.Velocity = -A.coeficient_dynamic_friction * A.Velocity;
Impulse_Correction_for_one(A);
}
