AnotherFoxGuy/bounce
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix comments and tolerances

Browse Source
This commit is contained in:
Irlan 2017-02-17 16:48:52 -02:00
parent 012460f6b0
commit a1fb4b903b
1 changed files with 25 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
src/bounce/collision/distance.cpp
View File

@ -157,13 +157,10 @@ void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2,
const b3Vec3& P1, const b3Vec3& N1, const b3Vec3& P1, const b3Vec3& N1,
const b3Vec3& P2, const b3Vec3& N2) const b3Vec3& P2, const b3Vec3& N2)
{ {
// sin^2 = 1 - cos^2 const float32 kTol = 0.001f;
// or
// sin = norm( cross(n1, n2) )
const float32 kTol = 0.0f;
float32 c = b3Dot(N1, N2); float32 b = b3Dot(N1, N2);
float32 den = 1.0f - c * c; float32 den = 1.0f - b * b;
if (den < kTol * kTol) if (den < kTol * kTol)
{ {
*C1 = P1; *C1 = P1;
@ -171,24 +168,15 @@ void b3ClosestPointsOnNormalizedLines(b3Vec3* C1, b3Vec3* C2,
return; return;
} }
den = 1.0f / den; float32 inv_den = 1.0f / den;
// a = dot(n1, e3)
// b = dot(n2, e3)
// c = dot(n1, n2)
// s - c * t = -dot(n1, e3)
// c * s - t = -dot(n2, e3)
// s = ( c * dot(n2, e3) - dot(n1, e3) ) / den
// t = ( dot(n2, e3) - c * dot(n1, e3) ) / den
b3Vec3 E3 = P1 - P2; b3Vec3 E3 = P1 - P2;
float32 a = b3Dot(N2, E3); float32 d = b3Dot(N1, E3);
float32 b = b3Dot(N1, E3); float32 e = b3Dot(N2, E3);
float32 s = den * (c * a - b); float32 s = inv_den * (b * e - d);
float32 t = den * (a - c - b); float32 t = inv_den * (e - b * d);
*C1 = P1 + s * N1; *C1 = P1 + s * N1;
*C2 = P2 + t * N2; *C2 = P2 + t * N2;
@ -234,11 +222,11 @@ void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
// or // or
// sin^2 = 1 - cos^2 // sin^2 = 1 - cos^2
// Zero parallelism tolerance used because the colinearity tolerance above is also zero. // Use small parallelism tolerance used because the coincidence tolerance above is also small/zero.
const float32 kTol = 0.0f; const float32 kTol = 0.001f;
float32 c = b3Dot(N1, N2); float32 b = b3Dot(N1, N2);
float32 den = 1.0f - c * c; float32 den = 1.0f - b * b;
if (den < kTol * kTol) if (den < kTol * kTol)
{ {
*C1 = P1; *C1 = P1;
@ -246,23 +234,23 @@ void b3ClosestPointsOnSegments(b3Vec3* C1, b3Vec3* C2,
} }
else else
{ {
// a = dot(n1, e3) // b = dot(n1, n2)
// b = dot(n2, e3) // d = dot(n1, e3)
// c = dot(n1, n2) // e = dot(n2, e3)
// s - c * t = -dot(n1, e3) // s - b * t = -d
// c * s - t = -dot(n2, e3) // b * s - t = -e
// s = ( c * dot(n2, e3) - dot(n1, e3) ) / den // s = ( b * e - d ) / den
// t = ( dot(n2, e3) - c * dot(n1, e3) ) / den // t = ( e - b * d ) / den
b3Vec3 E3 = P1 - P2; b3Vec3 E3 = P1 - P2;
float32 a = b3Dot(N2, E3); float32 d = b3Dot(N1, E3);
float32 b = b3Dot(N1, E3); float32 e = b3Dot(N2, E3);
float32 inv_den = 1.0f / den; float32 inv_den = 1.0f / den;
float32 s = inv_den * (c * a - b); float32 s = inv_den * (b * e - d);
float32 t = inv_den * (a - c - b); float32 t = inv_den * (e - b * d);
*C1 = P1 + s * N1; *C1 = P1 + s * N1;
*C2 = P2 + t * N2; *C2 = P2 + t * N2;