bool SATCollision(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
//test
//Vector3 Axis = new Vector3 (0, 0, 0);
// first we get the normals to test against
List <Vector3> Norms = new List <Vector3> ();
Norms.AddRange(GetNormals(body1));
Norms.AddRange(GetNormals(body2));
foreach (var axis in Norms)
{
var B1MAX = GetMaxAxis(body1, axis);
var B1MIN = GetMinAxis(body1, axis);
var B2MAX = GetMaxAxis(body2, axis);
var B2MIN = GetMinAxis(body2, axis);
if (B1MAX < B2MIN || B2MAX < B1MIN)
{
Normal = axis;
return(true);
}
}
// if no colisions on any axis
return(false);
}
bool SphereCollision(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
Normal = (body1.mCollider.Centre - body2.mCollider.Centre) / (body1.mCollider.Centre - body2.mCollider.Centre).magnitude;
bool test = (body1.mCollider.Centre - body2.mCollider.Centre).magnitude <= (body1.mCollider.Radius + body2.mCollider.Radius);
return(test);
}
List <Vector3> GetNormals(CRigidBody body)
{
List <Vector3> norms = new List <Vector3> ();
for (int i = 0; i < body.mCollider.Points.Count - 1; ++i)
{
var prenorm = body.mCollider.Points[i + 1] - body.mCollider.Points[i];
var leftnorm = new Vector3(prenorm.x, prenorm.y * -1, prenorm.z);
norms.Add(leftnorm.normalized);
}
return(norms);
}
void RestingResponse(CRigidBody body1, CRigidBody body2, Vector3 Normal)
{
return;
if (body1.CheckStaticFriction(Normal))
{
body1.ApplyFriction(Normal);
}
if (body2.CheckStaticFriction(Normal))
{
body2.ApplyFriction(Normal);
}
}
float GetMaxAxis(CRigidBody body, Vector3 Axis)
{
float max = 0;
foreach (var point in body.mCollider.Points)
{
var current = Vector3.Dot(point, Axis);
if (current > max)
{
max = current;
}
}
return(max);
}
float GetMinAxis(CRigidBody body, Vector3 Axis)
{
float min = 0;
foreach (var point in body.mCollider.Points)
{
var current = Vector3.Dot(point, Axis);
if (current < min)
{
min = current;
}
}
return(min);
}
void CollisionResponse(CRigidBody body1, CRigidBody body2, Vector3 Normal)
{
bool Test = false;
// deciede which type of response
var RelVel = RelativeVelocity (body1.mVelocity, body2.mVelocity,Normal);
Debug.Log (RelVel.ToString());
if(RelVel > 0 && RelVel < 1)
Test = !Test;
if (RelVel > 0)
return; // objects moving away no response needed
else if (RelVel < 0)
PenetratingResponse(body1, body2,RelVel,Normal);//objects colliding response needed
else if (RelVel == 0)
RestingResponse(body1,body2,Normal);// resting contact
}
bool NarrowphaseTest(CRigidBody body1, CRigidBody body2,ref Vector3 Normal)
{
if (body1.IsGround == true && body2.IsGround == true)
return false;
/*//check for square(AABB) to square
if (body1.mCollider.Type == Collision.CollisionType.AABB && body2.mCollider.Type == Collision.CollisionType.AABB)
return AABBCollision (body1, body2,ref Normal);
//else sphere to sphere
else if (body1.mCollider.Type == Collision.CollisionType.Sphere && body2.mCollider.Type == Collision.CollisionType.Sphere)
return SphereCollision (body1, body2,ref Normal);
else
return AABBSphereCollision (body1, body2,ref Normal);*/
return SATCollision (body1, body2,ref Normal);
}
bool AABBCollision(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
/*float distance = (body2.mCollider.Centre - body1.mCollider.Centre).magnitude - (body1.mCollider.Radius+body2.mCollider.Radius);
* Debug.Log (distance.ToString ());*/
if (body1.mCollider.Max.x > body2.mCollider.Min.x &&
body1.mCollider.Max.y > body2.mCollider.Min.y &&
body1.mCollider.Max.z > body2.mCollider.Min.z &&
body1.mCollider.Min.x < body2.mCollider.Max.x &&
body1.mCollider.Min.y < body2.mCollider.Max.y &&
body1.mCollider.Min.z < body2.mCollider.Max.z)
{
Normal = Norm(body1, body2);
return(true);
}
return(false);
}
bool NarrowphaseTest(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
if (body1.IsGround == true && body2.IsGround == true)
{
return(false);
}
/*//check for square(AABB) to square
* if (body1.mCollider.Type == Collision.CollisionType.AABB && body2.mCollider.Type == Collision.CollisionType.AABB)
* return AABBCollision (body1, body2,ref Normal);
* //else sphere to sphere
* else if (body1.mCollider.Type == Collision.CollisionType.Sphere && body2.mCollider.Type == Collision.CollisionType.Sphere)
* return SphereCollision (body1, body2,ref Normal);
* else
* return AABBSphereCollision (body1, body2,ref Normal);*/
return(SATCollision(body1, body2, ref Normal));
}
void PenetratingResponse(CRigidBody body1, CRigidBody body2, float RelVel, Vector3 Normal)
{
var CoefResti = (body1.mRestitution + body2.mRestitution) / 2;
//set numerator
var Numer = -(1 + CoefResti) * RelVel;
float Denom;
if (body1.mMass == 0)
{
Denom = 1 / body2.mMass;
}
else if (body2.mMass == 0)
{
Denom = 1 / body1.mMass;
}
else
{
Denom = 1 / body1.mMass + 1 / body2.mMass;
}
var Impulse = Numer / Denom;
body1.ApplyImpulse(-Normal * Impulse);
body2.ApplyImpulse(Normal * Impulse);
// friction
/*var RV = body2.mVelocity - body1.mVelocity;
* var tangent = RV - Vector3.Dot (RV, Normal) * Normal;
* tangent.Normalize ();
* float mag = -Vector3.Dot (RV,tangent) / (1/body1.mMass + 1/body2.mMass);
*
* float staticcoef = Mathf.Sqrt ((body1.mStaticFriction * body1.mStaticFriction) + (body2.mStaticFriction * body2.mStaticFriction));
* if (Mathf.Abs (mag) < Impulse * staticcoef)
* {
* body1.ApplyForce( mag * tangent);
* body2.ApplyForce(mag* -tangent);
* }
* else
* {
* float dynamiccoef = Mathf.Sqrt ((body1.mDynamicFriction * body1.mDynamicFriction) + (body2.mDynamicFriction * body2.mDynamicFriction));
* body1.ApplyForce( -Impulse * tangent * dynamiccoef);
* body2.ApplyForce(Impulse * -tangent * dynamiccoef);
* }*/
}
void CollisionResponse(CRigidBody body1, CRigidBody body2, Vector3 Normal)
{
bool Test = false;
// deciede which type of response
var RelVel = RelativeVelocity(body1.mVelocity, body2.mVelocity, Normal);
Debug.Log(RelVel.ToString());
if (RelVel > 0 && RelVel < 1)
{
Test = !Test;
}
if (RelVel > 0)
{
return; // objects moving away no response needed
}
else if (RelVel < 0)
{
PenetratingResponse(body1, body2, RelVel, Normal); //objects colliding response needed
}
else if (RelVel == 0)
{
RestingResponse(body1, body2, Normal); // resting contact
}
}
bool ClosestPoints(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
return true;
}
float GetMaxAxis(CRigidBody body,Vector3 Axis)
{
float max = 0;
foreach(var point in body.mCollider.Points)
{
var current = Vector3.Dot(point,Axis);
if(current > max)
max = current;
}
return max;
}
float GetMinAxis(CRigidBody body,Vector3 Axis)
{
float min = 0;
foreach(var point in body.mCollider.Points)
{
var current = Vector3.Dot(point,Axis);
if(current < min)
min = current;
}
return min;
}
List<Vector3> GetNormals (CRigidBody body)
{
List<Vector3> norms = new List<Vector3> ();
for(int i = 0; i<body.mCollider.Points.Count-1;++i)
{
var prenorm = body.mCollider.Points[i+1]-body.mCollider.Points[i];
var leftnorm = new Vector3(prenorm.x,prenorm.y*-1,prenorm.z);
norms.Add(leftnorm.normalized);
}
return norms;
}
bool SATCollision(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
//test
//Vector3 Axis = new Vector3 (0, 0, 0);
// first we get the normals to test against
List<Vector3> Norms = new List<Vector3> ();
Norms.AddRange (GetNormals (body1));
Norms.AddRange (GetNormals (body2));
foreach(var axis in Norms)
{
var B1MAX = GetMaxAxis(body1,axis);
var B1MIN = GetMinAxis(body1,axis);
var B2MAX = GetMaxAxis(body2,axis);
var B2MIN = GetMinAxis(body2,axis);
if(B1MAX < B2MIN || B2MAX < B1MIN)
{
Normal = axis;
return true;
}
}
// if no colisions on any axis
return false;
}
bool ClosestPoints(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
return(true);
}
bool BroadphaseTest(CRigidBody body1, CRigidBody body2)
{
// not implemented return true
return true;
}
Vector3 Norm(CRigidBody body1, CRigidBody body2)
{
//return (body1.mCollider.Position - body2.mCollider.Position) / (body1.mCollider.Position - body2.mCollider.Position).magnitude;
Vector3 Normal = new Vector3 ();
float penet = 0.0f;
// if we consider b1 on the bot and b2 on the top
var B1MAX = body1.mCollider.Max;
var B1MIN = body1.mCollider.Min;
var B2MAX = body2.mCollider.Max;
var B2MIN = body2.mCollider.Min;
bool left = false, right= false, top= false, bottom= false, front= false, back= false;
if (B1MAX.x > B2MIN.x) // check left vs right 1st
right = true;
if (B1MAX.y > B2MIN.y) // check up vs down
top = true;
if (B1MAX.z > B2MIN.z)
back = true;
if (B2MAX.z > B1MIN.z)
front = true;
if (B2MAX.y > B1MIN.y)
bottom = true;
if (B2MAX.x > B1MIN.x)
left = true;
// now find face with the LEAST penetration
if (left)
{
penet = Mathf.Abs(B2MAX.x - B1MIN.x);
Normal = new Vector3(-1,0,0);
}
if (top && penet > Mathf.Abs(B1MAX.y - B2MIN.y))
{
penet = Mathf.Abs(B1MAX.y - B2MIN.y);
Normal = new Vector3(0,1,0);
}
if (front && penet > Mathf.Abs(B2MAX.z - B1MIN.z))
{
penet = Mathf.Abs(B2MAX.z - B1MIN.z);
Normal = new Vector3(0,0,-1);
}
if (right && penet > Mathf.Abs(B1MAX.x - B2MIN.x))
{
penet = Mathf.Abs(B1MAX.x - B2MIN.x);
Normal = new Vector3(1,0,0);
}
if(bottom&& penet >Mathf.Abs(B2MAX.y - B1MIN.y))
{
penet = Mathf.Abs(B2MAX.y - B1MIN.y);
Normal = new Vector3(0,-1,0);
}
if(back&& penet >Mathf.Abs(B1MAX.z - B2MIN.z))
{
penet = Mathf.Abs(B1MAX.z - B2MIN.z);
Normal = new Vector3(0,0,1);
}
return Normal;
}
bool AABBCollision(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
/*float distance = (body2.mCollider.Centre - body1.mCollider.Centre).magnitude - (body1.mCollider.Radius+body2.mCollider.Radius);
Debug.Log (distance.ToString ());*/
if(body1.mCollider.Max.x > body2.mCollider.Min.x &&
body1.mCollider.Max.y > body2.mCollider.Min.y &&
body1.mCollider.Max.z > body2.mCollider.Min.z &&
body1.mCollider.Min.x < body2.mCollider.Max.x &&
body1.mCollider.Min.y < body2.mCollider.Max.y &&
body1.mCollider.Min.z < body2.mCollider.Max.z)
{
Normal = Norm(body1,body2);
return true;
}
return false;
}
Vector3 Norm(CRigidBody body1, CRigidBody body2)
{
//return (body1.mCollider.Position - body2.mCollider.Position) / (body1.mCollider.Position - body2.mCollider.Position).magnitude;
Vector3 Normal = new Vector3();
float penet = 0.0f;
// if we consider b1 on the bot and b2 on the top
var B1MAX = body1.mCollider.Max;
var B1MIN = body1.mCollider.Min;
var B2MAX = body2.mCollider.Max;
var B2MIN = body2.mCollider.Min;
bool left = false, right = false, top = false, bottom = false, front = false, back = false;
if (B1MAX.x > B2MIN.x) // check left vs right 1st
{
right = true;
}
if (B1MAX.y > B2MIN.y) // check up vs down
{
top = true;
}
if (B1MAX.z > B2MIN.z)
{
back = true;
}
if (B2MAX.z > B1MIN.z)
{
front = true;
}
if (B2MAX.y > B1MIN.y)
{
bottom = true;
}
if (B2MAX.x > B1MIN.x)
{
left = true;
}
// now find face with the LEAST penetration
if (left)
{
penet = Mathf.Abs(B2MAX.x - B1MIN.x);
Normal = new Vector3(-1, 0, 0);
}
if (top && penet > Mathf.Abs(B1MAX.y - B2MIN.y))
{
penet = Mathf.Abs(B1MAX.y - B2MIN.y);
Normal = new Vector3(0, 1, 0);
}
if (front && penet > Mathf.Abs(B2MAX.z - B1MIN.z))
{
penet = Mathf.Abs(B2MAX.z - B1MIN.z);
Normal = new Vector3(0, 0, -1);
}
if (right && penet > Mathf.Abs(B1MAX.x - B2MIN.x))
{
penet = Mathf.Abs(B1MAX.x - B2MIN.x);
Normal = new Vector3(1, 0, 0);
}
if (bottom && penet > Mathf.Abs(B2MAX.y - B1MIN.y))
{
penet = Mathf.Abs(B2MAX.y - B1MIN.y);
Normal = new Vector3(0, -1, 0);
}
if (back && penet > Mathf.Abs(B1MAX.z - B2MIN.z))
{
penet = Mathf.Abs(B1MAX.z - B2MIN.z);
Normal = new Vector3(0, 0, 1);
}
return(Normal);
}
bool BroadphaseTest(CRigidBody body1, CRigidBody body2)
{
// not implemented return true
return(true);
}
public void RemoveBody(CRigidBody oldbody)
{
mBodies.Remove(oldbody);
}
public void AddBody(CRigidBody newbody)
{
mBodies.Add(newbody);
}
public void AddBody(CRigidBody newbody)
{
mBodies.Add (newbody);
}
public void RemoveBody(CRigidBody oldbody)
{
mBodies.Remove (oldbody);
}
bool SphereCollision(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
Normal = (body1.mCollider.Centre - body2.mCollider.Centre)/ (body1.mCollider.Centre - body2.mCollider.Centre).magnitude;
bool test = (body1.mCollider.Centre - body2.mCollider.Centre).magnitude <= (body1.mCollider.Radius + body2.mCollider.Radius);
return test;
}
bool AABBSphereCollision(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
CRigidBody AABB, Sphere;
if (body1.mCollider.Type == Collision.CollisionType.AABB)
{
AABB = body1;
Sphere = body2;
}
else
{
AABB = body2;
Sphere = body1;
}
Vector3 N = AABB.mPosition - Sphere.mPosition;
Vector3 Closestpoint = N;
Closestpoint.x = Mathf.Clamp (Closestpoint.x,-AABB.mCollider.Radius, AABB.mCollider.Radius);
Closestpoint.y = Mathf.Clamp (Closestpoint.y,-AABB.mCollider.Radius, AABB.mCollider.Radius);
Closestpoint.z = Mathf.Clamp (Closestpoint.z,-AABB.mCollider.Radius, AABB.mCollider.Radius);
bool inside = false;
if(N == Closestpoint)
{
inside = true;
// find closest axis
if(Mathf.Abs(N.x) > Mathf.Abs(N.y) && Mathf.Abs(N.x) > Mathf.Abs(N.z))
{
// clam to nearest point
if(Closestpoint.x > 0)
Closestpoint.x = AABB.mCollider.Radius;
else
Closestpoint.x = -AABB.mCollider.Radius;
}
else if (Mathf.Abs(N.y) > Mathf.Abs(N.z))
{
if(Closestpoint.y > 0)
Closestpoint.y = AABB.mCollider.Radius;
else
Closestpoint.y = -AABB.mCollider.Radius;
}
else
{
if(Closestpoint.z > 0)
Closestpoint.z = AABB.mCollider.Radius;
else
Closestpoint.z = -AABB.mCollider.Radius;
}
}
Vector3 normal = N - Closestpoint;
float dist = normal.sqrMagnitude;
if (dist > Sphere.mCollider.Radius * Sphere.mCollider.Radius && !inside)
return false;
if (inside)
Normal = -N;
else
Normal = N;
/*var TL = AABB.mCollider.TL;
var BR = AABB.mCollider.BR;
var R = Sphere.mCollider.Radius;
var C = Sphere.mCollider.Centre;
var RSQ = R * R;
if(C.x<TL.x)
RSQ-=(C.x - TL.x)*(C.x - TL.x);
else if(C.x<BR.x)
RSQ-=(C.x - BR.x)*(C.x - BR.x);
if(C.y<TL.y)
RSQ-=(C.y - TL.y)*(C.y - TL.y);
else if(C.y<BR.y)
RSQ-=(C.y - BR.y)*(C.y - BR.y);
if(C.z<TL.z)
RSQ-=(C.z - TL.z)*(C.z - TL.z);
else if(C.z<BR.z)
RSQ-=(C.z - BR.z)*(C.z - BR.z);*/
return true;
}
bool AABBSphereCollision(CRigidBody body1, CRigidBody body2, ref Vector3 Normal)
{
CRigidBody AABB, Sphere;
if (body1.mCollider.Type == Collision.CollisionType.AABB)
{
AABB = body1;
Sphere = body2;
}
else
{
AABB = body2;
Sphere = body1;
}
Vector3 N = AABB.mPosition - Sphere.mPosition;
Vector3 Closestpoint = N;
Closestpoint.x = Mathf.Clamp(Closestpoint.x, -AABB.mCollider.Radius, AABB.mCollider.Radius);
Closestpoint.y = Mathf.Clamp(Closestpoint.y, -AABB.mCollider.Radius, AABB.mCollider.Radius);
Closestpoint.z = Mathf.Clamp(Closestpoint.z, -AABB.mCollider.Radius, AABB.mCollider.Radius);
bool inside = false;
if (N == Closestpoint)
{
inside = true;
// find closest axis
if (Mathf.Abs(N.x) > Mathf.Abs(N.y) && Mathf.Abs(N.x) > Mathf.Abs(N.z))
{
// clam to nearest point
if (Closestpoint.x > 0)
{
Closestpoint.x = AABB.mCollider.Radius;
}
else
{
Closestpoint.x = -AABB.mCollider.Radius;
}
}
else if (Mathf.Abs(N.y) > Mathf.Abs(N.z))
{
if (Closestpoint.y > 0)
{
Closestpoint.y = AABB.mCollider.Radius;
}
else
{
Closestpoint.y = -AABB.mCollider.Radius;
}
}
else
{
if (Closestpoint.z > 0)
{
Closestpoint.z = AABB.mCollider.Radius;
}
else
{
Closestpoint.z = -AABB.mCollider.Radius;
}
}
}
Vector3 normal = N - Closestpoint;
float dist = normal.sqrMagnitude;
if (dist > Sphere.mCollider.Radius * Sphere.mCollider.Radius && !inside)
{
return(false);
}
if (inside)
{
Normal = -N;
}
else
{
Normal = N;
}
/*var TL = AABB.mCollider.TL;
* var BR = AABB.mCollider.BR;
* var R = Sphere.mCollider.Radius;
* var C = Sphere.mCollider.Centre;
* var RSQ = R * R;
* if(C.x<TL.x)
* RSQ-=(C.x - TL.x)*(C.x - TL.x);
* else if(C.x<BR.x)
* RSQ-=(C.x - BR.x)*(C.x - BR.x);
* if(C.y<TL.y)
* RSQ-=(C.y - TL.y)*(C.y - TL.y);
* else if(C.y<BR.y)
* RSQ-=(C.y - BR.y)*(C.y - BR.y);
* if(C.z<TL.z)
* RSQ-=(C.z - TL.z)*(C.z - TL.z);
* else if(C.z<BR.z)
* RSQ-=(C.z - BR.z)*(C.z - BR.z);*/
return(true);
}
void RestingResponse(CRigidBody body1, CRigidBody body2, Vector3 Normal)
{
return;
if (body1.CheckStaticFriction (Normal))
body1.ApplyFriction (Normal);
if (body2.CheckStaticFriction (Normal))
body2.ApplyFriction (Normal);
}
void PenetratingResponse(CRigidBody body1, CRigidBody body2,float RelVel, Vector3 Normal)
{
var CoefResti = (body1.mRestitution+body2.mRestitution)/2;
//set numerator
var Numer = -(1+CoefResti)*RelVel;
float Denom;
if(body1.mMass == 0)
Denom = 1/body2.mMass;
else if(body2.mMass == 0)
Denom = 1/body1.mMass;
else
Denom = 1/body1.mMass + 1/body2.mMass;
var Impulse = Numer / Denom;
body1.ApplyImpulse (-Normal*Impulse);
body2.ApplyImpulse (Normal*Impulse);
// friction
/*var RV = body2.mVelocity - body1.mVelocity;
var tangent = RV - Vector3.Dot (RV, Normal) * Normal;
tangent.Normalize ();
float mag = -Vector3.Dot (RV,tangent) / (1/body1.mMass + 1/body2.mMass);
float staticcoef = Mathf.Sqrt ((body1.mStaticFriction * body1.mStaticFriction) + (body2.mStaticFriction * body2.mStaticFriction));
if (Mathf.Abs (mag) < Impulse * staticcoef)
{
body1.ApplyForce( mag * tangent);
body2.ApplyForce(mag* -tangent);
}
else
{
float dynamiccoef = Mathf.Sqrt ((body1.mDynamicFriction * body1.mDynamicFriction) + (body2.mDynamicFriction * body2.mDynamicFriction));
body1.ApplyForce( -Impulse * tangent * dynamiccoef);
body2.ApplyForce(Impulse * -tangent * dynamiccoef);
}*/
}
