/// <summary>
/// This function takes an array of edges that represent an oriented bounding box and another
/// oriented bounding box. The edges provided are clipped against the planes of the
/// provided bounding box.
/// </summary>
/// <param name="edges"></param>
/// <param name="obb"></param>
/// <returns></returns>
public static List <Point> ClipEdgesToOBB(Line[] edges, ColliderBox obb)
{
List <Point> result = new List <Point>();
Point intersection = new Point(Vector3.zero);
List <Plane> planes = GetPlanes(obb);
for (int i = 0; i < planes.Count; ++i)
{
//Debug.Log("Planes Count: " + planes.Count);
for (int j = 0; j < edges.Length; ++j)
{
//Debug.Log("Edges Count: " + edges.Length);
Tuple <Point, bool> info = ClipToPlane(planes[i], edges[j]);
intersection = info.Item1;
//Debug.Log("Point is inside: " + info.Item2 + " Point intersection: " + info.Item1.p);
if (info.Item2)
{
if (PointInOBB(intersection, obb))
{
result.Add(intersection);
}
}
}
}
return(result);
}
//---------------------------------
// OBB vs Sphere
//---------------------------------
private bool AreSphereOBBColliding(ColliderBox b, SphereCollider s, ColliderTypes collTypes = ColliderTypes.BothRigidBodies)
{
Tuple <Vector3, bool> infoClosestPoint = Util.FindClosestPoint(b, s);
this.currentClosestPointOnObb = infoClosestPoint.Item1;
bool sphereInsideObb = infoClosestPoint.Item2;
float distSphereObbPoint = (this.currentClosestPointOnObb - s._center).sqrMagnitude;
bool collision = distSphereObbPoint < s.SqrRadius;
if (collision || sphereInsideObb)
{
if (collTypes == ColliderTypes.BothRigidBodies)
{
this.CollisionResolutionObbSphere(b, s, sphereInsideObb);
}
else
{
this.CollisionResolutionObbWithSingleParticle(b, s, sphereInsideObb, collTypes);
}
}
return(collision || sphereInsideObb);
}
/// <summary>
/// Return true if the colliderboxes are colliding
/// </summary>
public bool AreOBBsColliding(ColliderBox b1, ColliderBox b2, bool cacheColResponse = false)
{
// Return no collision if the previous SA is still valid.
if (CheckPreviousSeparatingAxis(b1, b2))
{
return(false);
}
//Init
this.currentMinPenetrationDistance = float.MaxValue;
//this.currentMinPenetrationAxis = Vector3.zero;
Cube c1 = b1.cube;
Cube c2 = b2.cube;
// Get axis from first cube
Vector3 c1axis1 = c1.vertices[(int)CubeIdx.B] - c1.vertices[(int)CubeIdx.A];
Vector3 c1axis2 = c1.vertices[(int)CubeIdx.D] - c1.vertices[(int)CubeIdx.A];
Vector3 c1axis3 = c1.vertices[(int)CubeIdx.E] - c1.vertices[(int)CubeIdx.A];
// Get axis from second cube
Vector3 c2axis1 = c2.vertices[(int)CubeIdx.B] - c2.vertices[(int)CubeIdx.A];
Vector3 c2axis2 = c2.vertices[(int)CubeIdx.D] - c2.vertices[(int)CubeIdx.A];
Vector3 c2axis3 = c2.vertices[(int)CubeIdx.E] - c2.vertices[(int)CubeIdx.A];
Vector3[] axis = { c1axis1, c1axis2, c1axis3, c2axis1, c2axis2, c2axis3 };
// Check 6 axis from 2 cubes
foreach (Vector3 ax in axis)
{
if (SeparatingAxisCheck(b1, b2, ax, true, cacheColResponse, CollType.Vertex))
{
return(false);
}
}
// Check 9 axis given by cross product between 2 cubes
for (int i = 0; i < 3; i++)
{
Vector3 ax1 = Vector3.Cross(axis[i], axis[3]);
if (SeparatingAxisCheck(b1, b2, ax1, true, cacheColResponse, CollType.Edge))
{
return(false);
}
Vector3 ax2 = Vector3.Cross(axis[i], axis[4]);
if (SeparatingAxisCheck(b1, b2, ax2, true, cacheColResponse, CollType.Edge))
{
return(false);
}
Vector3 ax3 = Vector3.Cross(axis[i], axis[5]);
if (SeparatingAxisCheck(b1, b2, ax3, true, cacheColResponse, CollType.Edge))
{
return(false);
}
}
Logger.Instance.DebugInfo("No separating axis found (end of function AreOBBsColliding, returning true).", "COLLISION MANAGER");
// No separating axis found -> collision happened.
return(true);
}
public static ICollider GetLineOfSightObstruction(this RectF from, RectF to, IEnumerable <ICollider> obstacles, CastingMode castingMode = CastingMode.Rough)
{
var fromBox = new ColliderBox(from);
var toBox = new ColliderBox(to);
return(GetLineOfSightObstruction(fromBox, toBox, obstacles, castingMode));
}
/// <summary>
/// If collision happened project the ParticleObject back to prevCenterMass.
/// </summary>
/// <param name="b1"></param>
/// <param name="b2"></param>
public void CollisionResolutionOBB(CollisionManifold cm, ColliderBox b1, ColliderBox b2)
{
Vector3 currPoint = cm.avg_depth;
Vector3 projPoint = cm.avg_contact;
float dist = Vector3.Magnitude(currPoint - projPoint);
if (Util.CMP(dist, 0.0f) || float.IsNaN(dist))
{
return;
}
if (!b1.IsStatic() && !b2.IsStatic())
{
this.SeparateParticleObjects(b1.GetParticleObject(), currPoint, projPoint, b2.GetParticleObject());
}
else
{
if (!b1.IsStatic())
{
this.SeparateParticleObjects(b1.GetParticleObject(), currPoint, projPoint);
}
else if (!b2.IsStatic())
{
this.SeparateParticleObjects(b2.GetParticleObject(), projPoint, currPoint);
}
}
}
public void CollisionResolutionObbSphere(ColliderBox b, SphereCollider s, bool sphereInsideObb)
{
Vector3 temp = this.currentClosestPointOnObb - s._center;
float dirMult = sphereInsideObb ? -1 : 1;
Vector3 dir = temp.normalized * dirMult;
Vector3 currPoint = s._center + dir * s.Radius;
Vector3 projPoint = this.currentClosestPointOnObb;
if (!b.IsStatic() && !s.IsStatic())
{
this.SeparateParticleObjects(s.GetParticleObject(), currPoint, projPoint, b.GetParticleObject());
}
else
{
if (!s.IsStatic())
{
this.SeparateParticleObjects(s.GetParticleObject(), currPoint, projPoint);
}
else if (!b.IsStatic())
{
this.SeparateParticleObjects(b.GetParticleObject(), projPoint, currPoint);
}
}
}
public HitData IsOverlapping(Vector2 offset, ColliderBox other)
{
if (
offset.x +
this.transform.position.x - this.width / 2 <
other.transform.position.x + other.width / 2 &&
offset.x +
this.transform.position.x + this.width / 2 >
other.transform.position.x - other.width / 2 &&
offset.y +
this.transform.position.y - this.height / 2 <
other.transform.position.y + other.height / 2 &&
offset.y +
this.transform.position.y + this.height / 2 >
other.transform.position.y - other.height / 2
)
{
HitData hit = new HitData
{
overlap = new Vector2(
(this.width / 2 + other.width / 2) -
Mathf.Abs((this.transform.position.x + offset.x) - other.transform.position.x),
(this.height / 2 + other.height / 2) -
Mathf.Abs((this.transform.position.y + offset.y) - other.transform.position.y)
)
};
return(hit);
}
return(null);
}
public override void Start()
{
// создаем рабочий коллайдер. Без коллайдера объект не может учавствовать в физическом представлении
ColliderBox collider = new ColliderBox(new GalaxyVector3(0.4f, 0.4f, 0.4f));
physics.Activate(collider); // активируем физику
physics.mass = 1f; // устанавливае вес объекта в кг
transform.syncType = NetEntityAutoSync.position_and_rotation; // указываем способ синхронизации
}
private void CollisionResolutionObbWithSingleParticle(ColliderBox cb, SphereCollider s, bool sphereInsideObb, ColliderTypes collTypes)
{
Vector3 temp = this.currentClosestPointOnObb - s._center;
float dirMult = sphereInsideObb ? -1 : 1;
Vector3 dir = temp.normalized * dirMult;
Vector3 currPoint = s._center + dir * s.Radius;
Vector3 projPoint = this.currentClosestPointOnObb;
this.SeparateParticleObjectWithSingleParticles(cb, s, projPoint, currPoint, collTypes);
}
public static Point[] GetVertices(ColliderBox obb) // find vertices of OBB
{
Point[] vert_array = new Point[8]; // OBB always has 8 vertices
Cube obb_cube = obb.getOBBVertices();
for (int i = 0; i < obb_cube.vertices.Length; i++)
{
vert_array[i].p = obb_cube.vertices[i];
}
return(vert_array);
}
void Start()
{
colliderBox = GetComponent <ColliderBox>();
var wallObjects = GameObject.FindGameObjectsWithTag("Wall");
walls = new ColliderBox[wallObjects.Length];
for (int i = 0; i < walls.Length; i++)
{
walls[i] = wallObjects[i].GetComponent <ColliderBox>();
}
}
public bool CheckIn(ColliderBox cb)
{
Rect4 player = new Rect4(cb.size).Local2World(cb.transform);
Rect4 npc = new Rect4(check).Local2World(transform);
if (player.right <= npc.left || player.left >= npc.left || player.down >= npc.up || player.up <= npc.down)
{
return(false);
}
else
{
return(true);
}
}
// ===============
// CONSTRUCTOR
// ===============
public OctreeNode(OctreeNode parent, Vector3 thisChild_pos, float thisChild_halfLength, List <OctreeItem> potential_items, string name = "ROOT")
{
this.parent = parent;
this.halfDimensionLength = thisChild_halfLength;
this.pos = thisChild_pos;
this.name = name;
// Depth
if (parent != null)
{
this.nodeDepth = this.parent.nodeDepth + 1;
}
else
{
this.nodeDepth = 0;
}
// Octant Object and ColliderBox attachement (+ debug stuff)
octantGO = new GameObject();
octantGO.name = this.name;
this.colliderBoxNode = octantGO.AddComponent <ColliderBox>();
this.colliderBoxNode.AddToCollisionManager = false;
this.colliderBoxNode.xyzLength = Vector3.one;
octantGO.transform.position = this.pos;
octantGO.transform.localScale = Vector3.one * this.halfDimensionLength * 2;
this.colliderBoxNode.UpdateColliderPose(Vector3.zero);
// Debug text
GameObject octantGOChild = new GameObject("DebugText_" + this.name);
octantGOChild.transform.parent = octantGO.transform;
octantGOChild.transform.localPosition = Vector3.zero;
this.textDebugMesh = octantGOChild.AddComponent <TextMesh>();
this.textDebugMesh.fontSize = 50;
this.textDebugMesh.characterSize = 0.1f;
// Vizualization
//octantLineRenderer = octantGO.AddComponent<LineRenderer>();
//FillCube_VisualizeCoords(); // fill coords of line renderer
foreach (OctreeItem item in potential_items)
{
// Check if the item really belongs to this particular node
ProcessItem(item);
}
}
public Projectile(float width, float height, int damage = 0, float range = 500.0f, Type parentType = null) : base(width, height, Color4.White)
{
this.damage = damage;
this.range = range;
Velocity = Vector2.Zero;
ColliderBox = new ColliderBox(new Vector2(width, height), new Vector2(position), true);
if (!(parentType == null))
{
TypeChecksEnabled = true;
this.parentType = parentType;
}
else
{
TypeChecksEnabled = false;
}
}
public static Interval GetInterval(ColliderBox obb, Vector3 axis)
{
Point[] vertex = GetVertices(obb);
Interval result;
result.min = Vector3.Dot(axis, vertex[0].p);
result.max = Vector3.Dot(axis, vertex[0].p);
for (int i = 1; i < 8; ++i) // project all 8 vertices onto the axis and build interval
{
float projection = Vector3.Dot(axis, vertex[i].p);
result.min = (projection < result.min) ? projection : result.min;
result.max = (projection > result.max) ? projection : result.max;
}
return(result);
}
//---------------------------------
// OBB vs Sphere
//---------------------------------
public bool AreSphereOBBColliding(ColliderBox b, SphereCollider s)
{
Tuple <Vector3, bool> infoClosestPoint = Util.FindClosestPoint(b, s);
this.currentClosestPointOnObb = infoClosestPoint.Item1;
bool sphereInsideObb = infoClosestPoint.Item2;
float distSphereObbPoint = (this.currentClosestPointOnObb - s._center).sqrMagnitude;
bool collision = distSphereObbPoint < s.SqrRadius;
if (collision || sphereInsideObb)
{
this.CollisionResolutionObbSphere(b, s, sphereInsideObb);
}
return(collision || sphereInsideObb);
}
public static bool PointInOBB(Point point, ColliderBox obb)
{
// move point relative to obb by subtracting the obb center from the point
Vector3 dir = point.p - obb._center;
bool Build(Vector3 direction, Vector3 axis, float coord)
{
float distance = Vector3.Dot(direction, axis);
coord /= 2; // convert to half-extent
if (distance > coord + 0.0001f) // for example obb._xyzLength.x value, but half-extent value!
{
return(false);
}
if (distance < -coord - 0.0001f) // check in other direction
{
return(false);
}
return(true);
}
Point[] vertices = GetVertices(obb);
Vector3 x_axis = (vertices[(int)CubeIdx.D].p - vertices[(int)CubeIdx.A].p).normalized;
Vector3 y_axis = (vertices[(int)CubeIdx.E].p - vertices[(int)CubeIdx.A].p).normalized;
Vector3 z_axis = (vertices[(int)CubeIdx.B].p - vertices[(int)CubeIdx.A].p).normalized;
if (!Build(dir, x_axis, obb._xyzLength.x))
{
return(false);
}
if (!Build(dir, y_axis, obb._xyzLength.y))
{
return(false);
}
if (!Build(dir, z_axis, obb._xyzLength.z))
{
return(false);
}
return(true);
}
//TODO: check collisions between characters
private void checkCollisions()
{
foreach (Character character in characters)
{
foreach (Projectile projectile in projectiles)
{
if (projectile.TypeChecksEnabled && projectile.ParentType == character.GetType())
{
continue;
}
if (ColliderBox.CollidingAABB(projectile.ColliderBox, character.ColliderBox))
{
character.OnProjectileHit(projectile);
projectile.OnCharacterHit();
}
}
}
}
public void CollisionResolutionOBB(ColliderBox b1, ColliderBox b2)
{
// Find min penetration axis
AreOBBsColliding(b1, b2, true);
ShowCurrentSeparatingPlane();
// TODO: Assume the first object has the particle object (REMOVE THIS ASSUMPTION)
// CASE OF VERTEX
Logger.Instance.DebugInfo("COLLISION TYPE: " + this.currentCollType);
if (this.currentCollType == CollType.Vertex)
{
}
// TODO: ABOVE STEP Too slow: instead -> when searching for min axis and min distance [AreOBBsColliding(b1, b2, true);]
// -> try also to find the closest vertex to the plane/cube -> and then use that vertex to project our using the min axis/distance.
Vector3 currPoint = DebugSpheresContactObb[0];
Vector3 projPoint = currPoint + this.currentMinPenetrationAxis.normalized * this.currentMinPenetrationDistance;
if (!b1.IsStatic() && !b2.IsStatic())
{
this.SeparateParticleObjects(b1.GetParticleObject(), currPoint, projPoint, b2.GetParticleObject());
}
else
{
if (!b1.IsStatic())
{
this.SeparateParticleObjects(b1.GetParticleObject(), currPoint, projPoint);
}
else if (!b2.IsStatic())
{
this.SeparateParticleObjects(b2.GetParticleObject(), projPoint, currPoint);
}
}
Logger.Instance.DebugInfo("Updated particles, min axis dist: " + this.currentMinPenetrationDistance + ", min axis: " + this.currentMinPenetrationAxis, "INFO COLLISION RESOLUTION");
Logger.Instance.DebugInfo(this.currentMinPenetrationDistance.ToString());
Logger.Instance.DebugInfo(this.currentMinPenetrationAxis.ToString());
}
/// <summary>
/// Check if the previously detected SA is still a valid SA.
/// </summary>
public bool CheckPreviousSeparatingAxis(ColliderBox b1, ColliderBox b2)
{
int cacheId = this.GetCachedSeparatingAxisID(b1.Id, b2.Id);
// Check if this tuple is already cached
if (this.CachedSeparatingAxis.ContainsKey(cacheId))
{
// Check if the cached axis is still separating the two colliders
if (SeparatingAxisCheck(b1, b2, this.CachedSeparatingAxis[cacheId], false))
{
return(true);
}
else
{
this.CachedSeparatingAxis.Remove(cacheId);
return(false);
}
}
// No previous cached value
return(false);
}
/// <summary>
/// This function uses similar logic to testing if objects separate on a single axis in the SAT test
/// </summary>
/// <param name="obb1"></param>
/// <param name="obb2"></param>
/// <param name="axis"></param>
/// <param name="outShouldFlip"></param>
/// <returns></returns>
public static Tuple <float, bool> PenetrationDepth(ColliderBox obb1, ColliderBox obb2, Vector3 axis, bool outShouldFlip)
{
Tuple <float, bool> info = new Tuple <float, bool>(0.0f, outShouldFlip);
Interval i1 = GetInterval(obb1, axis.normalized);
Interval i2 = GetInterval(obb2, axis.normalized);
if (!((i2.min <= i1.max) && (i1.min <= i2.max)))
{
return(info); // No penetration
}
float len1 = i1.max - i1.min;
float len2 = i2.max - i2.min;
float min = Mathf.Min(i1.min, i2.min);
float max = Mathf.Max(i1.max, i2.max);
float length = max - min;
info.Item2 = (i2.min < i1.min);
info.Item1 = (len1 + len2) - length;
return(info);
}
public static Line[] GetEdges(ColliderBox obb) // find edges of OBB
{
Line[] edges = new Line[12]; // OBB always has 12 edges
//List<Line> edges = new List<Line>();
Point[] vertices = GetVertices(obb);
int i = 0;
void Build(CubeIdx From, CubeIdx To, int j)
{
int fromIndex = (int)From;
int toIndex = (int)To;
edges[j] = new Line(vertices[fromIndex].p, vertices[toIndex].p);
}
Build(CubeIdx.A, CubeIdx.B, i++); // A -> B
Build(CubeIdx.A, CubeIdx.D, i++); // A -> D
Build(CubeIdx.A, CubeIdx.E, i++); // A -> E
Build(CubeIdx.G, CubeIdx.F, i++); // G -> F
Build(CubeIdx.G, CubeIdx.C, i++); // G -> C
Build(CubeIdx.G, CubeIdx.H, i++); // G -> H
Build(CubeIdx.E, CubeIdx.F, i++); // E -> F
Build(CubeIdx.E, CubeIdx.H, i++); // E -> H
Build(CubeIdx.C, CubeIdx.B, i++); // C -> B
Build(CubeIdx.C, CubeIdx.D, i++); // C -> D
Build(CubeIdx.B, CubeIdx.F, i++); // B -> F
Build(CubeIdx.D, CubeIdx.H, i++); // D -> H
return(edges);
}
public static List <Plane> GetPlanes(ColliderBox obb) // find planes of OBB
{
Vector3 c = obb._center; // center
Vector3 l = obb._xyzLength / 2; // half-extents
Point[] vertices = GetVertices(obb);
// compute directions on all axis
Vector3 z_dir = (vertices[(int)CubeIdx.B].p - vertices[(int)CubeIdx.A].p).normalized;
Vector3 x_dir = (vertices[(int)CubeIdx.D].p - vertices[(int)CubeIdx.A].p).normalized;
Vector3 y_dir = (vertices[(int)CubeIdx.E].p - vertices[(int)CubeIdx.A].p).normalized;
List <Plane> result = new List <Plane>();
// build all the planes on the obb
result.Add(new Plane(x_dir, Vector3.Dot(x_dir, (c + x_dir * l.x))));
result.Add(new Plane(-x_dir, Vector3.Dot(-x_dir, (c - x_dir * l.x))));
result.Add(new Plane(y_dir, Vector3.Dot(y_dir, (c + y_dir * l.y))));
result.Add(new Plane(-y_dir, Vector3.Dot(-y_dir, (c - y_dir * l.y))));
result.Add(new Plane(z_dir, Vector3.Dot(z_dir, (c + z_dir * l.z))));
result.Add(new Plane(-z_dir, Vector3.Dot(-z_dir, (c - z_dir * l.z))));
return(result);
}
private void Start()
{
// Assign unique Id
this.AssignUniqueId();
// Check if this game object also has a colliderbox, if that's the case it means that its used for the octree
colliderBoxForOctree = this.GetComponent <ColliderBox>();
if (colliderBoxForOctree != null)
{
// make sure this is false (make it in the inspector to be sure)
this.colliderBoxForOctree.AddToCollisionManager = false;
}
// Add to collision manager
CollisionManager.Instance.AddCollider(this, this.isSingleParticle);
// Keep current scale, so if in game changes happen to scale, the collider will be scaled accordingly
this._initialMaxScale = Mathf.Max(this.transform.localScale.x, this.transform.localScale.y, this.transform.localScale.z);
this._assignedRadius = this.Radius;
this.isRunning = true;
this.SqrRadius = this.Radius * this.Radius;
Logger.Instance.DebugInfo("Radius: " + this.Radius.ToString() + "SqrRadius: " + this.SqrRadius.ToString(), "SPHERE COLLIDER");
}
public static CollisionManifold FindCollisionFeatures(ColliderBox obb1, ColliderBox obb2)
{
CollisionManifold result = new CollisionManifold();
//CollisionManifold.Reset(result);
result.Reset();
// First, make a quick collision test on spheres within the OBBs, if they don't collide => OBBs dont collide
Sphere s1 = new Sphere(new Point(obb1._center), Vector3.Magnitude(obb1._xyzLength / 2) + 0.1f);
Sphere s2 = new Sphere(new Point(obb2._center), Vector3.Magnitude(obb2._xyzLength / 2) + 0.1f);
if (!SphereSphere(s1, s2))
{
return(result);
}
Cube c1 = obb1.cube;
Cube c2 = obb2.cube;
List <Vector3> axis = new List <Vector3>();
// Get axis from first cube
Vector3 c1axis1 = (c1.vertices[(int)CubeIdx.B] - c1.vertices[(int)CubeIdx.A]).normalized;
Vector3 c1axis2 = (c1.vertices[(int)CubeIdx.D] - c1.vertices[(int)CubeIdx.A]).normalized;
Vector3 c1axis3 = (c1.vertices[(int)CubeIdx.E] - c1.vertices[(int)CubeIdx.A]).normalized;
axis.Add(c1axis1);
axis.Add(c1axis2);
axis.Add(c1axis3);
// Get axis from second cube
Vector3 c2axis1 = (c2.vertices[(int)CubeIdx.B] - c2.vertices[(int)CubeIdx.A]).normalized;
Vector3 c2axis2 = (c2.vertices[(int)CubeIdx.D] - c2.vertices[(int)CubeIdx.A]).normalized;
Vector3 c2axis3 = (c2.vertices[(int)CubeIdx.E] - c2.vertices[(int)CubeIdx.A]).normalized;
axis.Add(c2axis1);
axis.Add(c2axis2);
axis.Add(c2axis3);
// Check 9 axis given by cross product between 2 cubes
for (int i = 0; i < 3; ++i)
{
axis.Add(Vector3.Cross(axis[i], axis[3]));
axis.Add(Vector3.Cross(axis[i], axis[4]));
axis.Add(Vector3.Cross(axis[i], axis[5]));
}
Vector3[] test = axis.ToArray();
Vector3 hitNormal = new Vector3(0.0f, 0.0f, 0.0f);
bool shouldFlip = false;
for (int i = 0; i < test.Length; ++i) // axis.Count = 15
{
if (axis[i].x < 0.000001f)
{
test[i].x = 0.0f;
}
if (test[i].y < 0.000001f)
{
test[i].y = 0.0f;
}
if (test[i].z < 0.000001f)
{
test[i].z = 0.0f;
}
if (Vector3.Magnitude(test[i]) * Vector3.Magnitude(test[i]) < 0.001f)
{
continue;
}
Tuple <float, bool> pntrtion_info = PenetrationDepth(obb1, obb2, test[i], shouldFlip);
float depth = pntrtion_info.Item1;
shouldFlip = pntrtion_info.Item2;
//Debug.Log("shouldFlip" + shouldFlip);
//if (depth < 0.0f) return result; // if penetration depth < 0.0f
if (depth <= 0.001f)
{
Debug.Log("DEPTH: " + depth);
return(result);
}
else if (depth < result.depth)
{
if (shouldFlip)
{
test[i] = test[i] * -1.0f;
}
result.depth = depth;
hitNormal = test[i];
//Debug.Log("hitNormal" + hitNormal);
}
}
//Debug.Log("PenetrationDepth: " + result.depth);
//Debug.Log("shouldFlip: " + shouldFlip);
//Debug.Log("COLLIDING2: " + result.colliding);
//if (Util.CMP(hitNormal.magnitude, 0.0f))// return result; //if (hitNormal.magnitude <= 0.1f)
//{
// Debug.Log("RETURN hitNormal.magnitude: " + hitNormal.magnitude);
// return result;
//}
Vector3 axs = hitNormal.normalized;
List <Point> list1 = ClipEdgesToOBB(GetEdges(obb2), obb1);
List <Point> list2 = ClipEdgesToOBB(GetEdges(obb1), obb2);
result.contacts = new List <Vector3>();
result.depths = new List <Vector3>();
foreach (Point point in list1)
{
result.contacts.Add(point.p);
result.depths.Add(point.p);
}
foreach (Point point in list2)
{
result.contacts.Add(point.p);
result.depths.Add(point.p);
}
Interval interval = GetInterval(obb1, axs);
float depth_distance_points = (interval.max - interval.min) * 0.5f; // depth points after intersection inside obb
float contact_points = (interval.max - interval.min) * 0.5f - result.depth; // points of intersection at the surface
Vector3 pointOnPlane1 = obb1._center + axs * depth_distance_points;
Vector3 pointOnPlane2 = obb1._center + axs * contact_points;
for (int i = result.contacts.Count - 1; i >= 0; --i)
{
Vector3 contact = result.contacts[i];
result.contacts[i] = contact + (axs * Vector3.Dot(axs, pointOnPlane2 - contact));
result.depths[i] = contact + (axs * Vector3.Dot(axs, pointOnPlane1 - contact));
//result.depths.Add(contact + (axs * Vector3.Dot(axs, pointOnPlane1 - contact)));
// This bit is in the "There is more" section of the book
for (int j = result.contacts.Count - 1; j > i; --j)
{
float magnitude = Vector3.Magnitude(result.contacts[j] - result.contacts[i]);
if (magnitude * magnitude < 0.0001f)
{
result.contacts.RemoveAt(j);
result.depths.RemoveAt(j);
break;
}
}
}
foreach (Vector3 cp in result.contacts)
{
result.avg_contact += cp;
}
result.avg_contact /= result.contacts.Count;
//result.avg_depth = result.avg_contact + result.depth * result.normal;
foreach (Vector3 dp in result.depths)
{
result.avg_depth += dp;
}
result.avg_depth /= result.depths.Count;
result.colliding = true;
result.normal = axs;
return(result);
}
// ============================
// COLLISION
// ============================
/// <summary>
/// Finds the closest point on the OBB given another point.
/// </summary>
/// <param name="b"></param>
/// <param name="s"></param>
/// <returns></returns>
public static Tuple <Vector3, bool> FindClosestPoint(ColliderBox b, SphereCollider s)
{
Cube c = b.cube;
Vector3 cAxis1 = c.vertices[(int)CubeIdx.D] - c.vertices[(int)CubeIdx.A]; // local x
Vector3 cAxis2 = c.vertices[(int)CubeIdx.E] - c.vertices[(int)CubeIdx.A]; // local y
Vector3 cAxis3 = c.vertices[(int)CubeIdx.B] - c.vertices[(int)CubeIdx.A]; // local z
Vector3[] axes = { cAxis1.normalized, cAxis2.normalized, cAxis3.normalized };
Vector3 halfLenghts = b._xyzLength / 2f;
// Represent sphere position from obbs origin
bool centerIsInsideObb = true;
Vector3 closestPoint = b._center;
Vector3 point = s._center - b._center;
for (int i = 0; i < axes.Length; i++)
{
// Project the point and
float projValue = Vector3.Dot(point, axes[i]);
// See if point is bigger than half extents of obb
float halfLength = halfLenghts[i];
//Logger.Instance.DebugInfo("Proj Value " + i + ": " + projValue + ", HalfLen: " + halfLength, "SPHERE-OBB CHECK");
// Manual clamping
if (projValue < -halfLength)
{
centerIsInsideObb = false;
projValue = -halfLength;
}
else if (projValue > halfLength)
{
centerIsInsideObb = false;
projValue = halfLength;
}
closestPoint += axes[i] * projValue;
}
// If center is inside Obb we have to find the nearest face and project the point there (need for collision resolution)
if (centerIsInsideObb)
{
float min = float.MaxValue;
int idx = 0;
float faceDir = 1;
for (int i = 0; i < axes.Length; i++)
{
float projValue = Vector3.Dot(point, axes[i]);
float halfLength = halfLenghts[i];
float diff = Mathf.Min(min, halfLength - Mathf.Abs(projValue));
if (min > diff)
{
faceDir = Mathf.Sign(projValue);
min = diff;
idx = i;
}
}
closestPoint += axes[idx] * faceDir * min;
}
return(new Tuple <Vector3, bool>(closestPoint, centerIsInsideObb));
}
/// <summary>
/// Is a point inside this node?
/// </summary>
//public bool ContainsItemPos(Vector3 itemPos)
//{
// if (itemPos.x > pos.x + halfDimensionLength || itemPos.x < pos.x - halfDimensionLength)
// return false;
// if (itemPos.y > pos.y + halfDimensionLength || itemPos.y < pos.y - halfDimensionLength)
// return false;
// if (itemPos.z > pos.z + halfDimensionLength || itemPos.z < pos.z - halfDimensionLength)
// return false;
// return true;
//}
public bool ContainsItemColliderBox(ColliderBox b1)
{
return(CollisionManager.Instance.AreOBBsColliding(this.colliderBoxNode, b1));
}
/// <summary>
/// Checks if there is a separating axis between the two colliders.
/// Returns true if there is a separating axis (no collision).
/// </summary>
/// <param name="cacheAxis"> If the given axis is separating cache this in the system (faster for next round).
/// This is False if we need to convalidate the previous stored one (don't add 2 times).</param>
/// <param name="cacheColResolution"> When active we store the MTV and MTD. </param>
/// <param name="ax"></param>
/// <returns></returns>
private bool SeparatingAxisCheck(ColliderBox b1, ColliderBox b2, Vector3 ax, bool cacheAxis = true, bool cacheColResolution = false, CollType collType = CollType.Vertex)
{
if (ax == Vector3.zero)
{
return(false);
}
Vector3 axis = ax;
axis.Normalize();
Cube c1 = b1.cube;
Cube c2 = b2.cube;
float c1Max = float.MinValue;
float c1Min = float.MaxValue;
float c2Max = float.MinValue;
float c2Min = float.MaxValue;
// Project points from cubes to ax. Find extreme points in the axis.
for (int i = 0; i < 8; i++)
{
// Project point to axis;
float c1Proj = Vector3.Dot(c1.vertices[i], axis);
float c2Proj = Vector3.Dot(c2.vertices[i], axis);
c1Max = Mathf.Max(c1Max, c1Proj);
c1Min = Mathf.Min(c1Min, c1Proj);
c2Max = Mathf.Max(c2Max, c2Proj);
c2Min = Mathf.Min(c2Min, c2Proj);
}
var max = Mathf.Max(c1Max, c2Max);
var min = Mathf.Min(c1Min, c2Min);
float longSpan = max - min;
float overlapSpan = c1Max - c1Min + c2Max - c2Min;
bool noCollision = longSpan > overlapSpan;
// if no collision happened (found a separating axis) cache the separating axis
if (noCollision && cacheAxis && !cacheColResolution)
{
CachedSeparatingAxis.Add(GetCachedSeparatingAxisID(b1.Id, b2.Id), axis);
}
// we already know a collision happened and try to find min penetration axis for projection
if (cacheColResolution && !noCollision)
{
float val = overlapSpan - longSpan;
if (val >= 0 && val < this.currentMinPenetrationDistance && axis != Vector3.zero)
{
this.currentMinPenetrationDistance = val;
this.currentMinPenetrationAxis = axis;
this.currentCollType = collType;
Debug.Log("FOUND MIN AXIS");
Debug.Log(val);
Debug.Log(axis);
}
}
return(noCollision);
}
public static Cube GetOBBVertices(ColliderBox obb) // find vertices of OBB as Cube information
{
return(obb.getOBBVertices());
}
/// <summary>
/// Checks and resolves the collision between particle object colliders.
/// </summary>
private void CollidersCollisions()
{
int count = Colliders.Count;
for (int i = 0; i < count; i++)
{
for (int j = i + 1; j < count; j++)
{
// OBB vs OBB
if (Colliders[i] as ColliderBox != null && Colliders[j] as ColliderBox != null)
{
CollisionManifold features = Geometry.FindCollisionFeatures((ColliderBox)Colliders[i], (ColliderBox)Colliders[j]);
if (!features.colliding)
{
//Debug.Log("No collision happened.");
}
else
{
this.CollisionResolutionOBB(features, (ColliderBox)Colliders[i], (ColliderBox)Colliders[j]);
//Debug.Log("COLLISION: collision happened.");
}
}
// SPHERE vs SPHERE
else if (Colliders[i] as SphereCollider != null && Colliders[j] as SphereCollider != null)
{
if (AreSpheresColliding((SphereCollider)Colliders[i], (SphereCollider)Colliders[j]))
{
Logger.Instance.DebugInfo("Collision happened [Sphere vs Sphere]: " +
Colliders[i].Id + " - " +
Colliders[j].Id + " !",
"COLLISION_MANAGER");
}
}
// OBB vs SPHERE
else
{
ColliderBox b = Colliders[i] as ColliderBox;
SphereCollider s = Colliders[j] as SphereCollider;
if (b == null)
{
b = Colliders[j] as ColliderBox;
s = Colliders[i] as SphereCollider;
}
if (b != null && s != null)
{
if (AreSphereOBBColliding(b, s))
{
Logger.Instance.DebugInfo("Collision happened [OBB vs Sphere]: " +
Colliders[i].Id + " - " +
Colliders[j].Id + " !",
"COLLISION_MANAGER");
}
}
}
}
}
}
