private bool Gjk(Collider colliderA, Collider colliderB, out ContactData contactData)
{
contactData = default;
Vec3f a, b, c, d;
Vec3f Asa, Asb, Asc, Asd, Bsa, Bsb, Bsc, Bsd;
a = b = c = d = Vec3f.Zero;
Asa = Asb = Asc = Asd = Vec3f.Zero;
Bsa = Bsb = Bsc = Bsd = Vec3f.Zero;
Vec3f searchDir = colliderA.localCentroid - colliderB.localCentroid;
CsoSupport(colliderB, colliderA, searchDir, out c, out Asc, out Bsc);
searchDir = -c;
CsoSupport(colliderB, colliderA, searchDir, out b, out Asb, out Bsb);
if (Vec3f.Dot(b, searchDir) < 0)
{
return(false);
}
Vec3f bc = c - b;
searchDir = Vec3f.Cross(Vec3f.Cross(bc, -b), bc); //search perpendicular to line segment towards origin
if (searchDir.SqrMagnitude < MathUtils.Epsilon)
{
searchDir = Vec3f.Cross(bc, Vec3f.Right); //normal with x-axis
if (Vec3f.Aproximates(searchDir, Vec3f.Zero))
{
searchDir = Vec3f.Cross(bc, -Vec3f.Forward); //normal with z-axis
}
}
int simplexDimension = 2;
for (int i = 0; i < 64; i++)
{
CsoSupport(colliderB, colliderA, searchDir, out a, out Asa, out Bsa);
if (Vec3f.Dot(a, searchDir) < 0)
{
return(false);
}
simplexDimension++;
if (simplexDimension == 3)
{
UpdateSimplex3(ref a, ref b, ref c, ref d, ref simplexDimension, ref searchDir, ref Asa, ref Asb, ref Asc, ref Asd
, ref Bsa, ref Bsb, ref Bsc, ref Bsd);
}
else if (UpdateSimplex4(ref a, ref b, ref c, ref d, ref simplexDimension, ref searchDir, ref Asa, ref Asb, ref Asc, ref Asd
, ref Bsa, ref Bsb, ref Bsc, ref Bsd))
{
contactData = Epa(colliderA, colliderB, a, b, c, d, Asa, Asb, Asc, Asd, Bsa, Bsb, Bsc, Bsd);
return(true);
}
}
return(false);
}
public static Quatf operator *(Quatf lhs, Quatf rhs)
{
Vec3f lxyz = lhs.XYZ;
Vec3f rxyz = rhs.XYZ;
return(new Quatf(
(rhs.w * lxyz) + (lhs.w * rxyz) + Vec3f.Cross(lxyz, rxyz),
(lhs.w * rhs.w) - Vec3f.Dot(lxyz, rxyz)));
}
public Vec3f Normal(int edgeIdx)
{
Vec3f d1 = GetEdgeDelta(edgeIdx);
Vec3f d2 = GetEdgeDelta(Next(edgeIdx));
Vec3f norm = Vec3f.Cross(d1, d2);
return(norm.Normalized());
}
public static Vec3f operator *(Quatf q, Vec3f v)
{
var QuatVector = new Vec3f(q.x, q.y, q.z);
var uv = Vec3f.Cross(QuatVector, v);
var uuv = Vec3f.Cross(QuatVector, uv);
uv *= (2.0f * q.w);
uuv *= 2.0f;
return(v + uv + uuv);
}
public static bool ContainsPoint(Vec3f a, Vec3f b, Vec3f c, Vec3f point)
{
a -= point;
b -= point;
c -= point;
Vec3f u = Vec3f.Cross(b, c);
Vec3f v = Vec3f.Cross(c, a);
Vec3f w = Vec3f.Cross(a, b);
return(Vec3f.Dot(u, v) >= 0f && Vec3f.Dot(u, w) > 0f);
}
public Vec3f Normal(int vIdx1, int vIdx2, int vIdx3)
{
var v1 = Points[vIdx1];
var v2 = Points[vIdx2];
var v3 = Points[vIdx3];
Vec3f d1 = v2 - v1;
Vec3f d2 = v3 - v1;
Vec3f norm = Vec3f.Cross(d1, d2);
return(norm.Normalized());
}
public ContactData(Vec3f worldPointA, Vec3f localPointA, Vec3f worldPointB, Vec3f localPointB, Vec3f normal, float depth)
{
this.localPointA = localPointA;
this.localPointB = localPointB;
this.worldPointA = worldPointA;
this.worldPointB = worldPointB;
this.depth = depth;
this.normal = normal;
if (normal.x >= 0.57735f)
{
tang1 = new Vec3f(normal.y, -normal.x, 0);
}
else
{
tang1 = new Vec3f(0.0f, normal.z, -normal.y);
}
tang1.Normalize();
tang2 = Vec3f.Cross(normal, tang1);
}
public bool SameDirEdges(int edge1, int edge2)
{
Vec3f d1 = GetEdgeDelta(edge1);
Vec3f d2 = GetEdgeDelta(edge2);
if (Vec3f.Dot(d1, d2) < 0)
{
return(false);
}
float c = Vec3f.Cross(d1, d2).MagnitudeSqrd();
const float kEdgeThresh = 0.01f;
if (c >= kEdgeThresh * kEdgeThresh)
{
return(false);
}
else
{
return(true);
}
}
public Plane(Vec3f a, Vec3f b, Vec3f c)
{
normal = Vec3f.Cross(a - b, a - c).Normalized();
distance = Vec3f.Dot(normal, a);
}
public Vec3f GetNormal() => Vec3f.Cross(b - a, c - a).Normalized();