Float3 CartesianToSpherical(Float3 direction)
{
Float3 result;
float rDist = XlRSqrt(MagnitudeSquared(direction));
result[0] = XlACos(direction[2] * rDist);
result[1] = XlATan2(direction[1], direction[0]);
result[2] = 1.0f / rDist;
return result;
}
float Vector2::Normalise()
{
float sqLen = MagnitudeSquared();
assert(sqLen != 0); // Why the f**k would you do this?
float len = Sqrt(sqLen);
float invLen = 1.0f / len;
x *= invLen;
y *= invLen;
return len;
}
void CVector::Normalise()
{
float fLength = MagnitudeSquared();
if(fLength > 0.0)
{
x /= fLength;
y /= fLength;
z /= fLength;
}
else
{
x = 1.0;
}
}
inline B32 GJKIntersection(const AABB& a, const AABB& b, CollisionInfo *collison) {
V3 aabbA[8], aabbB[8];
AABBToVertices(a, aabbA, sizeof(V3));
AABBToVertices(b, aabbB, sizeof(V3));
GJKState gjkState = {};
auto sim = &GetEngine()->physicsSimulation;
CollisionVisualization *vis = &sim->collisionVisualization;
vis->a = a;
vis->b = b;
V3 initalPoint = GJKArbitraryPolytopeSupport(aabbA, 8, sizeof(V3), aabbB, 8, sizeof(V3), V3(1.0f, 0.0f, 0.0f));
gjkState.simplex[gjkState.simplexCount++] = initalPoint;
vis->AddGJKStep(gjkState.simplex, gjkState.simplexCount);
gjkState.searchDirection= -initalPoint;
static const size_t GJK_MAX_ITERATIONS = 32;
size_t iterationCount = 0;
while (iterationCount < GJK_MAX_ITERATIONS) {
V3 p = GJKArbitraryPolytopeSupport(aabbA, 8, sizeof(V3), aabbB, 8, sizeof(V3), gjkState.searchDirection);
F32 dot = Dot(p, gjkState.searchDirection);
if (dot < 0.0f) return false;
gjkState.simplex[gjkState.simplexCount++] = p;
vis->AddGJKStep(gjkState.simplex, gjkState.simplexCount);
if (GJKUpdateSimplex(&gjkState)) {
assert(gjkState.simplexCount == 4);
V3 penetration = EPAFromGJKSimplex(gjkState.simplex, aabbA, 8, sizeof(V3), aabbB, 8, sizeof(V3));
//V3 penetration = V3(0.0f);
collison->penetrationVector = penetration;
return true;
}
if (MagnitudeSquared(gjkState.searchDirection) < 0.01f) {
assert(false);
return true;
}
iterationCount++;
}
if (iterationCount == GJK_MAX_ITERATIONS) {
LogError("GJK Reached Max Iterrations");
GetEngine()->isPaused = true;
}
return false;
}
void v3f::Normalise() // Vector becomes a unit vector
{
float magsquared = MagnitudeSquared();
if( math::isZero( magsquared ) )
{
Zero();
}
else
{
float factor = math::InvSqrt( magsquared );
x *= factor;
y *= factor;
z *= factor;
}
}
bool DistanceToSphereIntersection(
float& distance,
Float3 rayStart, Float3 rayDirection, float sphereRadiusSq)
{
/* Find the std::distance, along the ray that begins at 'rayStart' and continues in unit direction 'direction', to the
first intersection with the sphere centered at the origin and with radius 'radius'.
Returns 0 if there is no intersection */
auto d = Dot(-rayStart, rayDirection);
const Float3 closestPoint = rayStart + d * rayDirection;
const auto closestDistanceSq = MagnitudeSquared(closestPoint);
if (closestDistanceSq > sphereRadiusSq)
return false;
auto a = XlSqrt(sphereRadiusSq - closestDistanceSq);
distance = std::min(d-a, d+a);
return true;
}
float Vector3::Magnitude()
{
return (float) std::sqrt(MagnitudeSquared());
}
bool Vector2::IsZeroMagnitude() const
{
return (MagnitudeSquared() < EPSILON);
}
float Vector2::Magnitude() const
{
return Sqrt(MagnitudeSquared());
}
double Vector3::Magnitude() const
{
return sqrt( MagnitudeSquared() );
}
chain::Real Vector3::Magnitude() const
{
return CH_SQRT(MagnitudeSquared());
}
Quaternion Quaternion::Inverse() const
{
return Conjugate() / MagnitudeSquared();
}
double Vector4D<double>::Magnitude()
{
return sqrt(MagnitudeSquared());
}
Scalar Magnitude() const {
return std::sqrt(MagnitudeSquared());
}
T Magnitude()
{
return (std::sqrt(MagnitudeSquared()));
}
//TODO(Torin) For now we will just return a
//penetration vector but we need to generate a contact manifold in the future
inline V3 EPAFromGJKSimplex(V3 *initalSimplex, V3 *a, size_t countA, size_t strideA, V3 *b, size_t countB, size_t strideB) {
struct EPAEdge {
V3 a, b;
};
static const size_t MAX_EDGE_COUNT = 256;
static const size_t MAX_TRIANGLE_COUNT = 128;
EPATriangle triangles[MAX_TRIANGLE_COUNT] = {};
EPAEdge edges[MAX_EDGE_COUNT] = {};
size_t triangleCount = 0;
size_t edgeCount = 0;
auto& vis = GetEngine()->physicsSimulation.collisionVisualization;
auto AddTriangle = [&](V3 a, V3 b, V3 c) {
EPATriangle& triangle = triangles[triangleCount++];
triangle.a = a;
triangle.b = b;
triangle.c = c;
V3 ab = b - a;
V3 ac = c - a;
triangle.normal = Cross(ab, ac);
triangle.normal = Normalize(triangle.normal);
//assert(Dot(triangle.normal, -a) < 0.0f);
};
auto RemoveTriangle = [&](size_t i) {
assert(i < triangleCount);
triangles[i] = triangles[triangleCount - 1];
triangleCount -= 1;
};
auto AddOrRemoveEdge = [&](V3 a, V3 b) {
for (size_t i = 0; i < edgeCount; i++) {
auto& edge = edges[i];
if (Equals(edge.a, b) && Equals(edge.b, a)) {
edges[i] = edges[edgeCount - 1];
edgeCount--;
return;
}
}
EPAEdge& edge = edges[edgeCount++];
edge.a = a;
edge.b = b;
};
{ //Add inital simplex to the polytope
const V3& a = initalSimplex[3];
const V3& b = initalSimplex[2];
const V3& c = initalSimplex[1];
const V3& d = initalSimplex[0];
AddTriangle(b, c, d);
AddTriangle(a, b, d);
AddTriangle(a, d, c);
AddTriangle(a, c, b);
EPATriangle dummy = {};
vis.AddEPAStep(dummy, triangles, triangleCount);
}
static size_t const MAX_ITERATIONS = 6;
size_t iterationCount = 0;
while (iterationCount < MAX_ITERATIONS) {
//Get closest face of the polytope to the origin
F32 minMagnitudeSquared = 999999.0f; //@TODO(Torin) FLT_MAX
size_t closestFaceIndex = 0;
for (size_t i = 0; i < triangleCount; i++) {
EPATriangle& t = triangles[i];
V3 closestPoint = EPAClosestPointToOrigin(t);
F32 magnitudeSquared = MagnitudeSquared(closestPoint);
if (magnitudeSquared < minMagnitudeSquared) {
minMagnitudeSquared = magnitudeSquared;
closestFaceIndex = i;
}
}
const EPATriangle closestTriangle = triangles[closestFaceIndex];
const V3 extendPoint = GJKArbitraryPolytopeSupport(a, countA, strideA, b, countB, strideB, closestTriangle.normal);
V3 closestPointOnTriangle = EPAClosestPointOnTriangle(extendPoint, closestTriangle);
if (MagnitudeSquared(Abs(extendPoint - closestPointOnTriangle)) < 0.01f) {
//This is the closest face to the mankowski difference
V3 penetrationVector = Project(closestPointOnTriangle, closestTriangle.normal);
vis.penetrationVector = penetrationVector;
return penetrationVector;
} else {
//Expand the Polytope
//TODO(Torin) @HACK How should this actualy be done?
B8 markedForRemoval[128];
memset(markedForRemoval, 0x00, 128);
assert(triangleCount < 128);
for (size_t i = 0; i < triangleCount; i++) {
EPATriangle triangle = triangles[i];
F32 dot = Dot(triangle.normal, extendPoint - triangle.a);
if (dot > 0.0f) {
markedForRemoval[i] = true;
AddOrRemoveEdge(triangle.a, triangle.b);
AddOrRemoveEdge(triangle.b, triangle.c);
AddOrRemoveEdge(triangle.c, triangle.a);
}
}
//@Hack??? Mabye this isn't such a bad idea...
size_t newTriangleCount = 0;
for (size_t i = 0; i < triangleCount; i++) {
if (markedForRemoval[i] == false) {
triangles[newTriangleCount++] = triangles[i];
}
}
triangleCount = newTriangleCount;
for (size_t i = 0; i < edgeCount; i++) {
auto& edge = edges[i];
AddTriangle(edge.a, edge.b, extendPoint);
}
vis.AddEPAStep(closestTriangle, triangles, triangleCount);
edgeCount = 0;
}
iterationCount++;
}
if (iterationCount == MAX_ITERATIONS) {
LogError("EPA Reached max iterrations");
GetEngine()->isPaused = true;
return V3(0.0f);
}
}
float CVector::Magnitude() const
{
return sqrt(MagnitudeSquared());
}
float Vector4D<float>::Magnitude()
{
return sqrt(MagnitudeSquared());
}