bool jfCollisionDetector_x86::tryAxis(
const jfCollisionBox& one,
const jfCollisionBox& two,
jfVector3& axis,
const jfVector3& toCentre,
unsigned index,
// These values may be updated
jfReal& smallestPenetration,
unsigned& smallestCase
) const
{
// Make sure we have a normalized axis, and don't check almost parallel axes
if (axis.squareMagnitude() < 0.0001)
{
return true;
}
axis.normalize();
jfReal penetration = penetrationOnAxis(one, two, axis, toCentre);
if (penetration < 0)
{
return false;
}
if (penetration < smallestPenetration)
{
smallestPenetration = penetration;
smallestCase = index;
}
return true;
}
void jfRigidBody_x86::calculateTransformMatrix(jfMatrix4* transformMatrix,
const jfVector3& pos,
const jfQuaternion& orientation) const
{
//@ref: Millington p.195
transformMatrix->setElem(0, (1 - (2*orientation.getJ()*orientation.getJ()) -
(2*orientation.getK()*orientation.getK())));
transformMatrix->setElem(1, ((2*orientation.getI()*orientation.getJ()) -
(2*orientation.getR()*orientation.getK())));
transformMatrix->setElem(2, ((2*orientation.getI()*orientation.getK()) +
(2*orientation.getR()*orientation.getJ())));
transformMatrix->setElem(3, pos.getX());
transformMatrix->setElem(4,((2*orientation.getI()*orientation.getJ()) +
(2*orientation.getR()*orientation.getK())));
transformMatrix->setElem(5, ((1 - (2*orientation.getI()*orientation.getI())) -
(2*orientation.getK()*orientation.getK())));
transformMatrix->setElem(6, ((2*orientation.getJ()*orientation.getK()) -
(2*orientation.getR()*orientation.getI())));
transformMatrix->setElem(7, pos.getY());
transformMatrix->setElem(8, ((2*orientation.getI()*orientation.getK()) -
(2*orientation.getR()*orientation.getJ())));
transformMatrix->setElem(9, ((2*orientation.getJ()*orientation.getK()) +
(2*orientation.getR()*orientation.getI())));
transformMatrix->setElem(10, (1 - (2*orientation.getI()*orientation.getI()) -
(2*orientation.getJ()*orientation.getJ())));
transformMatrix->setElem(11, pos.getZ());
}
void jfVector3_x86::crossProduct(const jfVector3& vec, jfVector3* result) const
{
jfVector3_x86 tempResult;
tempResult.setX((m_Y*vec.getZ()) - (m_Z*vec.getY()));
tempResult.setY((m_Z*vec.getX()) - (m_X*vec.getZ()));
tempResult.setZ((m_X*vec.getY()) - (m_Y*vec.getX()));
(*result) = tempResult;
}
jfReal jfCollisionDetector_x86::penetrationOnAxis(
const jfCollisionBox& one,
const jfCollisionBox& two,
jfVector3& axis,
const jfVector3& toCentre
) const
{
jfIntersectionTester_x86 intersectionTester;
// Project the half-size of one onto axis
jfReal oneProject = intersectionTester.transformToAxis(one, axis);
jfReal twoProject = intersectionTester.transformToAxis(two, axis);
// Project this onto the axis
jfReal distance = jfRealAbs(toCentre.dotProduct(axis));
// Return the overlap (i.e. positive indicates
// overlap, negative indicates separation).
return (oneProject + twoProject - distance);
}
void jfCollisionDetector_x86::contactPoint(
const jfVector3& pOne,
const jfVector3& dOne,
jfReal oneSize,
const jfVector3& pTwo,
const jfVector3& dTwo,
jfReal twoSize,
// If this is true, and the contact point is outside
// the edge (in the case of an edge-face contact) then
// we use one's midpoint, otherwise we use two's.
bool useOne,
jfVector3* result) const
{
//TODO:Opportunity for CUDA?
jfVector3_x86 toSt, cOne, cTwo;
jfReal dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
jfReal denom, mua, mub;
smOne = dOne.squareMagnitude();
smTwo = dTwo.squareMagnitude();
dpOneTwo = dTwo.dotProduct(dOne);
pOne.subtract(pTwo, &toSt);
dpStaOne = dOne.dotProduct(toSt);
dpStaTwo = dTwo.dotProduct(toSt);
denom = (smOne * smTwo) - (dpOneTwo * dpOneTwo);
// Zero denominator indicates parrallel lines
if (jfRealAbs(denom) < 0.0001f) {
if(useOne)
{
(*result) = pOne;
return;
}
else
{
(*result) = pTwo;
return;
}
}
mua = ((dpOneTwo * dpStaTwo) - (smTwo * dpStaOne)) / denom;
mub = ((smOne * dpStaTwo) - (dpOneTwo * dpStaOne)) / denom;
// If either of the edges has the nearest point out
// of bounds, then the edges aren't crossed, we have
// an edge-face contact. Our point is on the edge, which
// we know from the useOne parameter.
if ((mua > oneSize) ||
(mua < -oneSize) ||
(mub > twoSize) ||
(mub < -twoSize))
{
if(useOne)
{
(*result) = pOne;
return;
}
else
{
(*result) = pTwo;
return;
}
}
else
{
//cOne = pOne + dOne * mua;
dOne.multiply(mua, &cOne);
cOne += pOne;
//cTwo = pTwo + dTwo * mub;
dTwo.multiply(mub, &cTwo);
cTwo += pTwo;
cOne *= 0.5;
cTwo *= 0.5;
cOne.add(cTwo, result);
}
}
void jfVector3_x86::addScaledVector(const jfVector3& v, jfReal scale)
{
m_X += (v.getX() * scale);
m_Y += (v.getY() * scale);
m_Z += (v.getZ() * scale);
}
void jfVector3_x86::operator-=(const jfVector3& v)
{
m_X -= v.getX();
m_Y -= v.getY();
m_Z -= v.getZ();
}
void jfVector3_x86::operator+=(const jfVector3& v)
{
m_X += v.getX();
m_Y += v.getY();
m_Z += v.getZ();
}
void jfVector3_x86::componentProduct(const jfVector3& vec, jfVector3* result) const
{
result->setX(m_X * vec.getX());
result->setY(m_Y * vec.getY());
result->setZ(m_Z * vec.getZ());
}
void jfVector3_x86::subtract(const jfVector3& vec, jfVector3* result) const
{
result->setX(m_X - vec.getX());
result->setY(m_Y - vec.getY());
result->setZ(m_Z - vec.getZ());
}
void jfVector3_x86::add(const jfVector3& vec, jfVector3* result) const
{
result->setX(m_X + vec.getX());
result->setY(m_Y + vec.getY());
result->setZ(m_Z + vec.getZ());
}
jfReal jfVector3_x86::dotProduct(const jfVector3& v) const
{
return ( (v.getX()*m_X) + (v.getY()*m_Y) + (v.getZ()*m_Z) );
}
void jfVector3_x86::componentProductUpdate(const jfVector3& v)
{
m_X *= v.getX();
m_Y *= v.getY();
m_Z *= v.getZ();
}
