#pragma once
#include "array.h"
template <class T, class DirType> //Should be shape types
Fixed<20> PenetrationAlongDir(const DirType& N, const Fixed<20> baseN, const Vec<20>& centerOffset, const T& shapeB)
{
auto minB = Fixed<20>::MAX();
for(u32 i=0; i<4; ++i)
{
auto newB = Dot<20>(N, centerOffset + shapeB.GetPoint(i));
minB = Min(minB, newB);
}
return baseN - minB;
}
template <class T1, class T2, class DirType>
void GetPenetrationSAT(DirType& bestNorm, Fixed<20>& bestPen, const Vec<20>& dist /*centerOffset*/, const T1& shapeA, const T2& shapeB)
{
bestPen = Fixed<20>::MAX();
for(u32 i=0; i<4; ++i)
{
const auto N = shapeB.GetSidePerp(i);
const auto baseN = Dot<20>(shapeB.GetPoint(i), N);
const auto newPen = PenetrationAlongDir(N, baseN, -dist, shapeA);
if (newPen < bestPen)
{
bestPen = newPen;
bestNorm = N;
if (newPen.val < 0) {return;}
}
}
for(u32 i=0; i<4; ++i)
{
const auto N = shapeA.GetSidePerp(i);
const auto baseN = Dot<20>(shapeA.GetPoint(i), N);
const auto newPen = PenetrationAlongDir(N, baseN, dist, shapeB);
if (newPen < bestPen)
{
bestPen = newPen;
bestNorm = -N;
if (newPen.val < 0) {return;}
}
}
}
//Will return pt2 if it is exactly on the line, but not pt1
template <class DirType>
bool GetLineIntersection(Vec<20>& rval, const DirType& N, const Fixed<20>& baseN, const Vec<20>& pt1, const Vec<20>& pt2)
{
const auto lastN = Dot<20>(pt1, N);
const auto nextN = Dot<20>(pt2, N);
//Point is exactly on side
if (nextN == baseN)
{
rval = pt2;
return true;
}
else if (((nextN>baseN) && (lastN<baseN)) || ((nextN<baseN) && (lastN>baseN)))
{
//Clip the two sides together to get intersection point. We use a waited sum of the points on either side
const auto W2 = Div<30>(baseN - nextN, lastN - nextN);
const auto W1 = Fixed<30>(1073741824 /*1.0*/) - W2;
assert(W2.val >= 0 && W1.val >= 0);
rval = VecMult<20>(pt2, W1) + VecMult<20>(pt1, W2);
return true;
}
return false;
}
template <class T1, class T2> //Should be shape types
bool FindContactPoints(Array<Vec<20>, 2>& Points, const Vec<30>& N, const Vec<20>& centerOffset, const T1& shapeA, const T2& shapeB)
{
//Figure out what side of a is being intersected. If none match the norm, return false (failure)
int aside = -1;
for(u32 i=0; i<4; ++i)
{
if (static_cast<decltype(N)>(shapeA.GetSidePerp(i)) == N)
{
aside = i; break;
}
}
if (aside < 0) {return false;}
//Array<Vec<20>, 2> Points;
u32 pi = 0;
const auto baseN = Dot<20>(shapeA.GetPoint(aside), N);
for(u32 i=0; i<4 && pi < 2; ++i)
{
//If there's an intersection, it will store in first argument and return true
if (GetLineIntersection(Points[pi], N, baseN, centerOffset + shapeB.GetPoint(i-1), centerOffset + shapeB.GetPoint(i)))
{ ++pi; }
}
//If no points were found return failure
if (pi <= 0) {return false;}
//Now make sure that both points are actually on the side, i.e. clip them to the corners if not
const auto baseT1 = Dot<20>(shapeA.GetPoint(aside), Rot90(N));
const auto baseT2 = Dot<20>(shapeA.GetPoint(aside+1), Rot90(N));
assert(baseT2 < baseT1);
for(u32 p=0; p<pi; ++p)
{
const auto newT = Dot<20>(Points[p], Rot90(N));
if (newT >= baseT1) {Points[p] = shapeA.GetPoint(aside);}
else if (newT <= baseT2) {Points[p] = shapeA.GetPoint(aside+1);}
}
//It is possible to only find one point if the shape has a corner just barely touching the other. In this case, just double it for now
//And the calling function will deal with it
if (pi == 1) {Points[pi++] = Points[0];}
assert(pi == 2);
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class T2> //Should be shape types
void ShapeCollisionTest(ContactList& contacts, CubeData* bodyA, PosVel* bodyB, Vec<20> offsetB, const T2& shapeB, eMassType massB)
{
assert(bodyA && bodyB);
const auto& shapeA = bodyA->shape();
const auto dist = (bodyB->pos + offsetB) - bodyA->pos;
Vec<30> bestNorm;
auto bestPen = Fixed<20>::MAX();
GetPenetrationSAT(bestNorm, bestPen, dist, shapeA, shapeB);
if (bestPen.val < 0) {return;}
Array<Vec<20>, 2> Points; //Contact points
bool done = false;
done = FindContactPoints(Points, -bestNorm, dist, shapeA, shapeB);
if (!done) //Try cutting against the sides of shape B if shape A didn't work
{
done = FindContactPoints(Points, bestNorm, -dist, shapeB, shapeA);
if (!done)
{
PRINT("!!!!!!!!!!!!!!!!!!!!!!!!!");
PRINT(bestNorm);
PRINT(dist);
PRINT(shapeA.GetPoint(0));
PRINT(shapeB.GetPoint(0));
PRINT(cube.dir);
}
assert(done);
Points[0] += dist; //FCP returns points in the reference frame of the first shape argument.
Points[1] += dist; //Since we passed b first, we have to move them back to a
}
contacts.addCubeContact(bodyA, bodyB, massB, bestNorm, -Points[0], -Points[1], bestPen);
}
template <class T2> //Should be shape types
void ShapeCollisionTest(ContactList& contacts, PickoryData* bodyA, PosVel* bodyB, Vec<20> offsetB, const T2& shapeB, eMassType massB)
{
assert(bodyA && bodyB);
const auto& shapeA = bodyA->shape();
const auto dist = (bodyB->pos + offsetB) - bodyA->pos;
//Vec<30> bestNorm;
UnitVec bestNorm;
auto bestPen = Fixed<20>::MAX();
GetPenetrationSAT(bestNorm, bestPen, dist, shapeA, shapeB);
if (bestPen.val < 0) {return;}
contacts.addContact(bodyA, bodyB, massB, bestNorm, bestPen);
}
template <class T2>
void DoShapeCollisionTests(ContactList& contacts, CubeData* bodyA1, PickoryData* bodyA2, PosVel* bodyB, Vec<20> offsetB, const T2& shapeB, eMassType massB)
{
ShapeCollisionTest(contacts, bodyA1, bodyB, offsetB, shapeB, massB);
ShapeCollisionTest(contacts, bodyA2, bodyB, offsetB, shapeB, massB);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void LineCollisionTest(ContactList& contacts, CubeData* bodyA, const Surface& s)
{
PosVel* const bodyB = &dummy;
assert(bodyA);
const auto& shapeA = bodyA->shape();
const auto T = s.dir;
const auto N = -Rot90(s.dir);
assert(T == Rot90(N));
const auto BBsize = Fixed<20>(22985121 /*21.92032*/); // Half width of a bounding square for the cube
const auto surfN = Dot<20>(s.pos, N);
const auto cubeN = Dot<20>(bodyA->pos, N);
//Bounding box check
if (cubeN >= surfN + BBsize) {return;}
if (cubeN < surfN) {return;} //If cube is already more than half way in, there's no real hope of resolving the collision, so just bail
const auto surfT1 = Dot<20>(s.pos, T);
const auto surfT2 = surfT1 + s.length;
const auto cubeT = Dot<20>(bodyA->pos, T);
//Bounding box check
if (cubeT <= surfT1 - BBsize) {return;}
if (cubeT >= surfT2 + BBsize) {return;}
auto bestPen = -Fixed<20>::MAX(); //Best penetration
Array<Vec<20>, 2> Points; //Contact points
u32 pi = 0;
//Clip each side of the cube to get the contact points and calculate penetration while we're at it
for(u32 i=0; i<4 && pi < 2; ++i)
{
const auto nextN = cubeN + Dot<20>(shapeA.GetPoint(i), N);
bestPen = Max(bestPen, surfN - nextN);
//If there's an intersection, it will store in first argument and return true
if (GetLineIntersection(Points[pi], N, surfN, bodyA->pos + shapeA.GetPoint(i-1), bodyA->pos + shapeA.GetPoint(i)))
{ ++pi; }
}
//Check if penetration is nonnegative
if(bestPen.val < 0) {return;}
if (pi == 1) {Points[pi++] = Points[0];}
assert(pi == 2);
const auto ptT1 = Dot<20>(Points[0], T);
const auto ptT2 = Dot<20>(Points[1], T);
//Check if the collision actually occured within the bounds of the line segment
if ((ptT1 <= surfT1 && ptT2 <= surfT1) || (ptT1 >= surfT2 && ptT2 >= surfT2)) {return;}
for(int pi=0; pi<2; ++pi)
{
const auto temp = Dot<20>(Points[pi], T);
if (temp <= surfT1) {Points[pi] = s.pos;}
else if (temp >= surfT2) {Points[pi] = s.pos + VecMult<20>(s.dir, s.length);}
}
contacts.addCubeContact(bodyA, bodyB, mtSTATIC, N, bodyA->pos - Points[0], bodyA->pos - Points[1], bestPen);
}
void LineCollisionTest(ContactList& contacts, PickoryData* bodyA, const Surface& s)
{
PosVel* const bodyB = &dummy;
assert(bodyA);
const auto& shapeA = bodyA->shape();
const auto T = s.dir;
const auto N = -Rot90(s.dir);
//calculate bounding box size along N and T
const auto boxN = Abs(shapeA.GetPoint(0) * N);
const auto boxT = Abs(shapeA.GetPoint(0) * T);
const auto surfN = Dot<20>(s.pos, N);
const auto kidN = Dot<20>(bodyA->pos, N);
//Bounding box check
if (kidN > surfN + boxN) {return;}
if (kidN < surfN) {return;} //If kid is already more than half way in, there's no real hope of resolving the collision, so just bail
const auto surfT1 = Dot<20>(s.pos, T);
const auto surfT2 = surfT1 + s.length;
const auto kidT = Dot<20>(bodyA->pos, T);
//Bounding box check along T
if (kidT <= surfT1 - boxT) {return;}
if (kidT >= surfT2 + boxT) {return;}
const auto bestPen = surfN - (kidN - boxN);
contacts.addContact(bodyA, bodyB, mtSTATIC, N, bestPen);
}
template <class T1> //should be CubeData or PickoryData
bool EdgeChainCollisionTest(ContactList& contacts, T1* bodyA, const Surface& s1, const Surface& s2)
{
assert(s2.pos == s1.end());
const auto& shapeA = bodyA->shape();
//Check if corner is convex
if (Rot90(s1.dir) == s2.dir)
{
//Check if corner is inside cube
if (shapeA.Contains(s2.pos - bodyA->pos))
{
const auto offset = half(s2.end() - s1.pos);
const auto center = s1.pos + offset;
const Rect r(offset);
ShapeCollisionTest(contacts, bodyA, &dummy, center, r, mtSTATIC);
return true;
}
}
//If we reach this point, we didn't do a corner check
//LineCollisionTest(contacts, bodyA, s1);
if (portals.Linked() && s1.portal)
{
ArrayList<interval, 4> intvs;
//Calculate pieces
for(u32 ptli=0; ptli<2; ++ptli)
{
if (portals[ptli].surf == &s1)
{
intvs.add(portals[ptli].left);
intvs.add(portals[ptli].right);
}
}
for(u32 ivi = 0; ivi<intvs.size(); ++ivi)
{
const auto& iv = intvs[ivi];
//if (iv.s2 == iv.s1) {continue;} //skip intervals with 0 length
const auto offset = VecMult<20>(s1.dir, half(iv.s2 - iv.s1)) + VecMult<20>(Rot90(s1.dir), Fixed<20>(16777216 /*16.0*/));
const auto center = s1.pos + offset + VecMult<20>(s1.dir, iv.s1);
const Rect r(offset);
ShapeCollisionTest(contacts, bodyA, &dummy, center, r, mtSTATIC);
}
if (intvs.size() > 0) {return false;}
}
LineCollisionTest(contacts, bodyA, s1);
return false;
}
|