public static void AABBOBBCollision(CollisionManager.HullCollision collision)
{
Matrix4x4 transformatA;
Matrix4x4 transformatB;
bool flip = false;
if (collision.a.GetComponent <Hull3D>().GetHullType() == CollisionHull3D.hullType.AABB)
{
transformatA = collision.a.GetComponent <Particle3D>().GetLocalToWorldtransform(false);
transformatB = collision.b.GetComponent <Particle3D>().GetLocalToWorldtransform(false);
}
else
{
transformatA = collision.b.GetComponent <Particle3D>().GetLocalToWorldtransform(false);
transformatB = collision.a.GetComponent <Particle3D>().GetLocalToWorldtransform(false);
}
Vector3 toCentre = collision.b.GetComponent <Particle3D>().position - collision.a.GetComponent <Particle3D>().position;
List <Vector3> axis = new List <Vector3>();
axis.Add(transformatB.GetColumn(0));
axis.Add(transformatB.GetColumn(1));
axis.Add(transformatB.GetColumn(2));
float bestOverlap = float.MaxValue;
int bestIndex;
bool colStatus = true;
for (int i = 0; i < axis.Count; i++)
{
if (axis[i].sqrMagnitude < 0.001f)
{
continue;
}
axis[i].Normalize();
float overlap = -1f;
if (!flip)
{
overlap = OverlapOnAxis(collision.a, collision.b, collision.a.GetComponent <AABBHull>().halfSize, collision.b.GetComponent <OBBHull>().halfSize, axis[i], toCentre);
}
if (flip)
{
overlap = OverlapOnAxis(collision.b, collision.a, collision.b.GetComponent <AABBHull>().halfSize, collision.a.GetComponent <OBBHull>().halfSize, axis[i], toCentre);
}
if (overlap < 0)
{
colStatus = false;
return;
}
if (overlap < bestOverlap)
{
bestOverlap = overlap;
bestIndex = i;
}
}
collision.status = colStatus;
}
public static void SphereSphereCollision(CollisionManager.HullCollision col)
{
// *IMPORTANT* for circle and square the collision only wirks with obejct1 - object 2 and not viceversa, must be a prob in clollision resolution
SphereHull hull1 = col.a.GetComponent <SphereHull>();
SphereHull hull2 = col.b.GetComponent <SphereHull>();
Vector3 range = (hull2.transform.position + hull2.localCenter) - (hull1.transform.position + hull1.localCenter); // make sure offsets arent screwing things up
float overlap = (hull2.radius + hull1.radius) - range.magnitude;
//HullCollision col = new CollisionManager.HullCollision();
//col.a = hull1;
//col.b = hull2;
col.penetration = range * overlap;
CollisionManager.HullCollision.Contact con0 = new CollisionManager.HullCollision.Contact();
con0.point = (range.normalized * hull1.radius) + hull1.transform.position;
con0.normal = range.normalized;
con0.restitution = Mathf.Min(hull1.restitution, hull2.restitution);
col.contacts[0] = con0;
Particle3D c1 = hull1.GetComponentInParent <Particle3D>();
Particle3D c2 = hull2.GetComponentInParent <Particle3D>();
Vector3 closingVel = c2.velocity - c1.velocity; // started as c1 -c2
col.closingVelocity = closingVel;
if (overlap >= 0)
{
col.status = true;
//Debug.Log("touch");
}
else
{
col.status = false;
}
}
public static void AABBAABBCollision(CollisionManager.HullCollision col)
{
Vector3 min0, max0, min1, max1;
AABBHull A = col.a.GetComponent <AABBHull>();
AABBHull B = col.b.GetComponent <AABBHull>();
min0 = A.transform.position - A.halfSize + A.localCenter;
max0 = A.transform.position + A.halfSize + A.localCenter;
min1 = B.transform.position - B.halfSize + B.localCenter;
max1 = B.transform.position + B.halfSize + B.localCenter;
Vector3 range = (B.transform.position + B.localCenter) - (A.transform.position + A.localCenter); // make sure offsets arent screwing things up
float xOverlap = A.halfSize.x + B.halfSize.x - Mathf.Abs(range.x);
float yOverlap = A.halfSize.y + B.halfSize.y - Mathf.Abs(range.y);
float zOverlap = A.halfSize.z + B.halfSize.z - Mathf.Abs(range.z); // was math.abs
col.penetration = new Vector3(xOverlap, yOverlap);
//Vector3 closingVel = A.velocity - B.velocity;
Vector3 closingVel = B.GetComponent <Particle3D>().velocity - A.GetComponent <Particle3D>().velocity;
col.closingVelocity = closingVel;
CollisionManager.HullCollision.Contact con0 = new CollisionManager.HullCollision.Contact();
con0.point = new Vector3(Mathf.Clamp(range.x, -A.halfSize.x, A.halfSize.x), Mathf.Clamp(range.y, -A.halfSize.y, A.halfSize.y), Mathf.Clamp(range.z, -A.halfSize.z, A.halfSize.z));
con0.restitution = Mathf.Min(A.restitution, B.restitution);
if (max0.x >= min1.x && max1.x >= min0.x)
{
if (max0.y >= min1.y && max1.y >= min0.y)
{
Vector3 collisionNormal = new Vector3();
if (con0.point.x == A.halfSize.x)//added mathf
{
collisionNormal = new Vector3(1.0f, 0.0f, 0.0f);
}
if (con0.point.x == -A.halfSize.x)//added mathf
{
collisionNormal = new Vector3(-1.0f, 0.0f, 0.0f);
}
if (con0.point.y == A.halfSize.y)
{
collisionNormal = new Vector3(0.0f, 1.0f, 0.0f);
}
if (con0.point.y == -A.halfSize.y)
{
collisionNormal = new Vector3(0.0f, -1.0f, 0.0f);
}
if (con0.point.z == A.halfSize.z)
{
collisionNormal = new Vector3(0.0f, 0.0f, 1.0f);
}
if (con0.point.z == -A.halfSize.z)
{
collisionNormal = new Vector3(0.0f, 0.0f, -1.0f);
}
con0.normal = collisionNormal;
col.status = true;
}
}
else
{
col.status = false;
}
col.contacts[0] = con0;
}
public static void SphereOBBCollision(CollisionManager.HullCollision col)
{
Particle3D sphereParticle;
Particle3D boxParticle;
OBBHull obbHull;
SphereHull sphereHull;
bool flip = false;
if (col.a.GetComponent <Hull3D>().GetHullType() == CollisionHull3D.hullType.Sphere)
{
// Debug.Log("unflipped");
obbHull = col.b.GetComponent <OBBHull>();
sphereHull = col.a.GetComponent <SphereHull>();
boxParticle = col.b.GetComponent <Particle3D>();
sphereParticle = col.a.GetComponent <Particle3D>();
}
else
{
flip = true;
//Debug.Log("flipped");
obbHull = col.a.GetComponent <OBBHull>();
sphereHull = col.b.GetComponent <SphereHull>();
boxParticle = col.a.GetComponent <Particle3D>();
sphereParticle = col.b.GetComponent <Particle3D>();
}
Vector3 closestPoint;
Vector3 localRange = boxParticle.GetWorldToLocaltransform(false).MultiplyPoint(sphereParticle.position) - boxParticle.GetWorldToLocaltransform(false).MultiplyPoint(boxParticle.position);
//Vector3 range = (sphereParticle.position) - (boxParticle.position);
closestPoint = new Vector3(Mathf.Clamp(localRange.x, -obbHull.halfSize.x, obbHull.halfSize.x),
Mathf.Clamp(localRange.y, -obbHull.halfSize.y, obbHull.halfSize.y),
Mathf.Clamp(localRange.z, -obbHull.halfSize.z, obbHull.halfSize.z));
col.contacts[0].point = boxParticle.GetLocalToWorldtransform(false).MultiplyPoint(closestPoint);
Vector3 closingVel = sphereParticle.GetComponent <Particle3D>().velocity - boxParticle.GetComponent <Particle3D>().velocity; // swapped
col.closingVelocity = closingVel;
//if(closingVel.magnitude < 0f)
Vector3 collisionNormal = new Vector3();
/*
* if (closestPoint.x == obbHull.halfSize.x)//added mathf
* collisionNormal = new Vector3(1.0f, 0.0f, 0.0f);
* if (closestPoint.x == -obbHull.halfSize.x)//added mathf
* collisionNormal = new Vector3(-1.0f, 0.0f, 0.0f);
* if (closestPoint.y == obbHull.halfSize.y)
* collisionNormal = new Vector3(0.0f, 1.0f, 0.0f);
* if (closestPoint.y == -obbHull.halfSize.y)
* collisionNormal = new Vector3(0.0f, -1.0f, 0.0f);
* if (closestPoint.z == obbHull.halfSize.z)
* collisionNormal = new Vector3(0.0f, 0.0f, 1.0f);
* if (closestPoint.z == -obbHull.halfSize.z)
* collisionNormal = new Vector3(0.0f, 0.0f, -1.0f);
*/
if (closestPoint.x == obbHull.halfSize.x)//added mathf
{
collisionNormal.x = 1.0f;
}
if (closestPoint.x == -obbHull.halfSize.x)//added mathf
{
collisionNormal.x = -1.0f;
}
if (closestPoint.y == obbHull.halfSize.y)
{
collisionNormal.y = 1.0f;
}
if (closestPoint.y == -obbHull.halfSize.y)
{
collisionNormal.y = -1.0f;
}
if (closestPoint.z == obbHull.halfSize.z)
{
collisionNormal.z = 1.0f;
}
if (closestPoint.z == -obbHull.halfSize.z)
{
collisionNormal.z = -1.0f;
}
collisionNormal.Normalize();
Vector3 penetration = localRange - closestPoint;
col.penetration = penetration;
col.contacts[0].normal = (boxParticle.GetLocalToWorldtransform(false).MultiplyPoint(collisionNormal)).normalized;
//Debug.DrawLine(col.contacts[0].point, boxParticle.GetLocalToWorldtransform(false).MultiplyPoint(collisionNormal));
col.contacts[0].restitution = Mathf.Min(obbHull.restitution, sphereHull.restitution);
/*
* if (flip)
* {
* CollisionManager.HullCollision swapCol = new CollisionManager.HullCollision();
* swapCol.a = col.a;
* swapCol.b = col.b;
* col.b = swapCol.a;
* col.a = swapCol.b;
* }
*/
//Debug.DrawLine(col.contacts[0].point, boxParticle.GetLocalToWorldtransform(false).MultiplyPoint(penetration) );
if (penetration.magnitude <= sphereParticle.GetComponent <SphereHull>().radius)
{
col.status = true;
}
else
{
col.status = false;
}
}
//public hullType hull;
public static void ResolveCollision(CollisionManager.HullCollision col)
{
//Debug.Log("Trying to resolve collision");
Particle3D A = col.a.GetComponent <Particle3D>();
Particle3D B = col.b.GetComponent <Particle3D>();
/*
* if(col.a.GetHullType() == hullType.OBB && col.b.GetHullType() == hullType.Sphere)
* {
* A = col.a.GetComponent<Particle3D>();
* B = col.b.GetComponent<Particle3D>();
* }
* else if (col.a.GetHullType() == hullType.Sphere && col.b.GetHullType() == hullType.OBB)
* {
* A = col.b.GetComponent<Particle3D>();
* B = col.a.GetComponent<Particle3D>();
* }*/
float invAMass;
float invBMass;
if (A.mass == 0)
{
invAMass = 0;
}
else
{
invAMass = 1 / A.mass;
}
if (B.mass == 0)
{
invBMass = 0;
}
else
{
invBMass = 1 / B.mass;
}
float velAlongNormal = Vector3.Dot(col.contacts[0].normal, col.closingVelocity.normalized);
//Debug.Log("velAlongNormal " + velAlongNormal);
if (velAlongNormal > 0)
{
return; // > makes square work properly
}
//Debug.Log(velAlongNormal);
// restitustion
float e = col.contacts[0].restitution;
// impulse scalar
float j = -(1 + e) * velAlongNormal;
j /= invAMass + invBMass;
//Debug.Log(j);
Vector3 impulse = j * col.contacts[0].normal;
//Debug.Log("impules = " + impulse);
// Positional Correction
if (col.status)
{
//A.velocity = new Vector3(0.0f, 0.0f, 0.0f);
//B.velocity = new Vector3(0.0f, 0.0f, 0.0f);
A.velocity -= invAMass * impulse;
B.velocity += invBMass * impulse;
float percent = 0.2f;
float slop = 0.01f;
Vector3 correction = Mathf.Max(velAlongNormal - slop, 0) / (invAMass + invBMass) * percent * col.contacts[0].normal;
A.position -= invAMass * correction; // started -
B.position += invBMass * correction; // started +
//col.resolved = true;
}
if (col.wasStatus && !col.status)
{
col.resolved = true;
}
col.wasStatus = col.status;
}
public static void SphereAABBCollision(CollisionManager.HullCollision collision)
{
AABBHull boxHull;
SphereHull sphere;
if (collision.a.GetHullType() == hullType.AABB)
{
boxHull = collision.a.GetComponent <AABBHull>();
sphere = collision.b.GetComponent <SphereHull>();
}
else
{
//Debug.Log("flipped");
boxHull = collision.b.GetComponent <AABBHull>();
sphere = collision.a.GetComponent <SphereHull>();
}
Vector3 closestPoint;
Vector3 range = (boxHull.transform.position + boxHull.localCenter) - (sphere.transform.position + sphere.localCenter);
closestPoint = new Vector3(Mathf.Clamp(range.x, -boxHull.halfSize.x, boxHull.halfSize.x), Mathf.Clamp(range.y, -boxHull.halfSize.y, boxHull.halfSize.y), Mathf.Clamp(range.z, -boxHull.halfSize.z, boxHull.halfSize.z));
Vector3 closingVel = sphere.GetComponent <Particle3D>().velocity - boxHull.GetComponent <Particle3D>().velocity; // swapped
Vector3 penetration = closestPoint - range; // swapped
collision.closingVelocity = closingVel;
collision.penetration = penetration;
CollisionManager.HullCollision.Contact con0 = new CollisionManager.HullCollision.Contact();
con0.point = closestPoint;
con0.restitution = Mathf.Min(boxHull.restitution, sphere.restitution);
if (penetration.magnitude <= sphere.radius)
{
Vector3 collisionNormal = new Vector3();
if (closestPoint.x == boxHull.halfSize.x)//added mathf
{
collisionNormal.x = 1.0f;
}
if (closestPoint.x == -boxHull.halfSize.x)//added mathf
{
collisionNormal.x = -1.0f;
}
if (closestPoint.y == boxHull.halfSize.y)
{
collisionNormal.y = 1.0f;
}
if (closestPoint.y == -boxHull.halfSize.y)
{
collisionNormal.y = -1.0f;
}
if (closestPoint.z == boxHull.halfSize.z)
{
collisionNormal.z = 1.0f;
}
if (closestPoint.z == -boxHull.halfSize.z)
{
collisionNormal.z = -1.0f;
}
collisionNormal.Normalize();
con0.normal = -collisionNormal;
//Debug.Log(con0.normal);
collision.status = true;
collision.contacts[0] = con0;
}
else
{
collision.status = false;
}
}