dgFloat32 dgCollisionBVH::RayCast (const dgVector& localP0, const dgVector& localP1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
if (PREFILTER_RAYCAST (preFilter, body, this, userData)) {
return dgFloat32 (1.2f);
}
dgFloat32 param = dgFloat32 (1.2f);
dgBVHRay ray (localP0, localP1);
ray.m_t = 2.0f;
ray.m_me = this;
ray.m_userData = userData;
if (!m_userRayCastCallback) {
ForAllSectorsRayHit (ray, RayHit, &ray);
if (ray.m_t <= 1.0f) {
param = ray.m_t;
contactOut.m_normal = ray.m_normal.Scale (dgRsqrt ((ray.m_normal % ray.m_normal) + 1.0e-8f));
contactOut.m_userId = ray.m_id;
}
} else {
if (body) {
ray.m_matrix = body->m_collisionWorldMatrix;
}
ForAllSectorsRayHit (ray, RayHitUser, &ray);
if (ray.m_t <= 1.0f) {
param = ray.m_t;
contactOut.m_normal = ray.m_normal.Scale (dgRsqrt ((ray.m_normal % ray.m_normal) + 1.0e-8f));
contactOut.m_userId = ray.m_id;
}
}
return param;
}
dgFloat32 dgCollisionChamferCylinder::RayCastSimd (const dgVector& q0, const dgVector& q1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
if (PREFILTER_RAYCAST (preFilter, body, this, userData)) {
return dgFloat32 (1.2f);
}
// dgFloat32 t = dgFloat32 (1.2f);
if (q0.m_x > m_height) {
if (q1.m_x < m_height) {
dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector (dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
if (q0.m_x < -m_height) {
if (q1.m_x > -m_height) {
dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector (dgFloat32 (-1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
contactOut.m_userId = SetUserDataID();
return t1;
}
}
}
return dgCollisionConvex::RayCastSimd (q0, q1, contactOut, NULL, NULL, NULL);
}
dgFloat32 dgCollisionUserMesh::RayCast (
const dgVector& localP0,
const dgVector& localP1,
dgContactPoint& contactOut,
OnRayPrecastAction preFilter,
const dgBody* const body,
void* const userData) const
{
dgFloat32 t;
dgFloat32 param;
if (PREFILTER_RAYCAST (preFilter, body, this, userData)) {
return dgFloat32 (1.2f);
}
param = dgFloat32 (1.2f);
if (m_rayHitCallBack) {
dgCollisionMeshRayHitDesc data;
data.m_localP0 = localP0;
data.m_localP1 = localP1;
data.m_userData = m_userData;
data.m_altenateUserData = userData;
if (body) {
data.m_matrix = body->m_collisionWorldMatrix;
}
t = m_rayHitCallBack (data);
if ((t < dgFloat32 (1.0f)) && (t > dgFloat32 (0.0f))) {
param = t;
contactOut.m_normal = data.m_normal;
contactOut.m_userId = data.m_userId;
}
}
return param;
}