dgFloat32 dgBody::RayCast (const dgLineBox& line, OnRayCastAction filter, OnRayPrecastAction preFilter, void* const userData, dgFloat32 maxT) const
{
dgAssert (filter);
dgVector l0 (line.m_l0);
dgVector l1 (line.m_l0 + (line.m_l1 - line.m_l0).Scale4 (dgMin(maxT, dgFloat32 (1.0f))));
if (dgRayBoxClip (l0, l1, m_minAABB, m_maxAABB)) {
// if (1) {
//l0 = dgVector (-20.3125000f, 3.54991579f, 34.3441200f, 0.0f);
//l1 = dgVector (-19.6875000f, 3.54257250f, 35.2211456f, 0.0f);
dgContactPoint contactOut;
const dgMatrix& globalMatrix = m_collision->GetGlobalMatrix();
dgVector localP0 (globalMatrix.UntransformVector (l0));
dgVector localP1 (globalMatrix.UntransformVector (l1));
dgVector p1p0 (localP1 - localP0);
if ((p1p0 % p1p0) > dgFloat32 (1.0e-12f)) {
dgFloat32 t = m_collision->RayCast (localP0, localP1, dgFloat32 (1.0f), contactOut, preFilter, this, userData);
if (t < dgFloat32 (1.0f)) {
dgVector p (globalMatrix.TransformVector(localP0 + (localP1 - localP0).Scale3(t)));
dgVector l1l0 (line.m_l1 - line.m_l0);
t = ((p - line.m_l0) % l1l0) / (l1l0 % l1l0);
if (t < maxT) {
dgAssert (t >= dgFloat32 (0.0f));
dgAssert (t <= dgFloat32 (1.0f));
contactOut.m_normal = globalMatrix.RotateVector (contactOut.m_normal);
maxT = filter (this, contactOut.m_collision0, p, contactOut.m_normal, contactOut.m_shapeId0, userData, t);
}
}
}
}
return maxT;
}
void RayCastCompoundsAllSubShapes(const dVector& origin, const dVector& end)
{
m_param = 1.0f;
m_body = NULL;
NewtonWorld* const world = GetWorld();
NewtonWorldRayCast(world, &origin[0], &end[0], PickCompound, this, NULL, 0);
if (m_body) {
// we found a compound, find all sub shape on teh path of the ray
dMatrix matrix;
NewtonBodyGetMatrix(m_body, &matrix[0][0]);
dVector localP0(matrix.UntransformVector(origin));
dVector localP1(matrix.UntransformVector(end));
NewtonCollision* const compoundCollision = NewtonBodyGetCollision(m_body);
dAssert(NewtonCollisionGetType(compoundCollision) == SERIALIZE_ID_COMPOUND);
for (void* node = NewtonCompoundCollisionGetFirstNode(compoundCollision); node; node = NewtonCompoundCollisionGetNextNode(compoundCollision, node)) {
dVector normal;
dLong attribute;
NewtonCollision* const subShape = NewtonCompoundCollisionGetCollisionFromNode(compoundCollision, node);
dFloat xxx = NewtonCollisionRayCast(subShape, &localP0[0], &localP1[0], &normal[0], &attribute);
if (xxx < 1.0f) {
dTrace (("sub shape hit\n"))
}
}
}
}
