Require C++ Sweep Test code converted to C#?
#include "vector.h"
template< class T >
inline void SWAP( T& a, T& b )
//swap the values of a and b
{
const T temp = a;
a = b;
b = temp;
}
// Return true if r1 and r2 are real
inline bool QuadraticFormula
(
const SCALAR a,
const SCALAR b,
const SCALAR c,
SCALAR& r1, //first
SCALAR& r2 //and second roots
)
{
const SCALAR q = b*b - 4*a*c;
if( q >= 0 )
{
const SCALAR sq = sqrt(q);
const SCALAR d = 1 / (2*a);
r1 = ( -b + sq ) * d;
r2 = ( -b - sq ) * d;
return true;//real roots
}
else
{
return false;//complex roots
}
}
const bool SphereSphereSweep
(
const SCALAR ra, //radius of sphere A
const VECTOR& A0, //previous position of sphere A
const VECTOR& A1, //current position of sphere A
const SCALAR rb, //radius of sphere B
const VECTOR& B0, //previous position of sphere B
const VECTOR& B1, //current position of sphere B
SCALAR& u0, //normalized time of first collision
SCALAR& u1 //normalized time of second collision
)
{
const VECTOR va = A1 - A0;
//vector from A0 to A1
const VECTOR vb = B1 - B0;
//vector from B0 to B1
const VECTOR AB = B0 - A0;
//vector from A0 to B0
const VECTOR vab = vb - va;
//relative velocity (in normalized time)
const SCALAR rab = ra + rb;
const SCALAR a = vab.dot(vab);
//u*u coefficient
const SCALAR b = 2*vab.dot(AB);
//u coefficient
const SCALAR c = AB.dot(AB) - rab*rab;
//constant term
//check if they're currently overlapping
if( AB.dot(AB) <= rab*rab )
{
u0 = 0;
u1 = 0;
return true;
}
//check if they hit each other
// during the frame
if( QuadraticFormula( a, b, c, u0, u1 ) )
{
if( u0 > u1 )
SWAP( u0, u1 );
return true;
}
return false;
}