int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride) { dxTriMesh* TriMesh = (dxTriMesh*)g1; dReal Length = dGeomRayGetLength(RayGeom); int FirstContact, BackfaceCull; dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); int ClosestHit = dGeomRayGetClosestHit(RayGeom); dVector3 Origin, Direction; dGeomRayGet(RayGeom, Origin, Direction); char intersect=0; GIM_TRIANGLE_RAY_CONTACT_DATA contact_data; if(ClosestHit) { intersect = gim_trimesh_ray_closest_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); } else { intersect = gim_trimesh_ray_collision(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); } if(intersect == 0) { return 0; } int OutTriCount = 0; if(TriMesh->RayCallback) { if(TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v)) { OutTriCount = 1; } } else { OutTriCount = 1; } if(OutTriCount>0) { dContactGeom* Contact = SAFECONTACT(Flags, Contacts, (OutTriCount-1), Stride); VEC_COPY(Contact->pos,contact_data.m_point); VEC_COPY(Contact->normal,contact_data.m_normal); Contact->depth = contact_data.tparam; Contact->g1 = TriMesh; Contact->g2 = RayGeom; } return OutTriCount; }
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride) { dIASSERT (Stride >= (int)sizeof(dContactGeom)); dIASSERT (g1->type == dTriMeshClass); dIASSERT (RayGeom->type == dRayClass); dIASSERT ((Flags & NUMC_MASK) >= 1); dxTriMesh* TriMesh = (dxTriMesh*)g1; dReal Length = dGeomRayGetLength(RayGeom); int FirstContact, BackfaceCull; dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); int ClosestHit = dGeomRayGetClosestHit(RayGeom); dVector3 Origin, Direction; dGeomRayGet(RayGeom, Origin, Direction); char intersect=0; GIM_TRIANGLE_RAY_CONTACT_DATA contact_data; if(ClosestHit) { intersect = gim_trimesh_ray_closest_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); } else { intersect = gim_trimesh_ray_collisionODE(&TriMesh->m_collision_trimesh,Origin,Direction,Length,&contact_data); } if(intersect == 0) { return 0; } if(!TriMesh->RayCallback || TriMesh->RayCallback(TriMesh, RayGeom, contact_data.m_face_id, contact_data.u , contact_data.v)) { dContactGeom* Contact = &( Contacts[ 0 ] ); VEC_COPY(Contact->pos,contact_data.m_point); VEC_COPY(Contact->normal,contact_data.m_normal); Contact->depth = contact_data.tparam; Contact->g1 = TriMesh; Contact->g2 = RayGeom; Contact->side1 = contact_data.m_face_id; Contact->side2 = -1; return 1; } return 0; }
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){ dIASSERT (Stride >= (int)sizeof(dContactGeom)); dIASSERT (g1->type == dTriMeshClass); dIASSERT (RayGeom->type == dRayClass); dIASSERT ((Flags & NUMC_MASK) >= 1); dxTriMesh* TriMesh = (dxTriMesh*)g1; const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind(); dIASSERT(uiTLSKind == RayGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); RayCollider& Collider = pccColliderCache->_RayCollider; dReal Length = dGeomRayGetLength(RayGeom); int FirstContact, BackfaceCull; dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); int ClosestHit = dGeomRayGetClosestHit(RayGeom); Collider.SetFirstContact(FirstContact != 0); Collider.SetClosestHit(ClosestHit != 0); Collider.SetCulling(BackfaceCull != 0); Collider.SetMaxDist(Length); dVector3 Origin, Direction; dGeomRayGet(RayGeom, Origin, Direction); /* Make Ray */ Ray WorldRay; WorldRay.mOrig.x = Origin[0]; WorldRay.mOrig.y = Origin[1]; WorldRay.mOrig.z = Origin[2]; WorldRay.mDir.x = Direction[0]; WorldRay.mDir.y = Direction[1]; WorldRay.mDir.z = Direction[2]; /* Intersect */ Matrix4x4 amatrix; int TriCount = 0; if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) { TriCount = pccColliderCache->Faces.GetNbFaces(); } if (TriCount == 0) { return 0; } const CollisionFace* Faces = pccColliderCache->Faces.GetFaces(); int OutTriCount = 0; for (int i = 0; i < TriCount; i++) { if (TriMesh->RayCallback == null || TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID, Faces[i].mU, Faces[i].mV)) { const int& TriIndex = Faces[i].mFaceID; if (!Callback(TriMesh, RayGeom, TriIndex)) { continue; } dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); dVector3 dv[3]; FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); dVector3 vu; vu[0] = dv[1][0] - dv[0][0]; vu[1] = dv[1][1] - dv[0][1]; vu[2] = dv[1][2] - dv[0][2]; vu[3] = REAL(0.0); dVector3 vv; vv[0] = dv[2][0] - dv[0][0]; vv[1] = dv[2][1] - dv[0][1]; vv[2] = dv[2][2] - dv[0][2]; vv[3] = REAL(0.0); dCROSS(Contact->normal, =, vv, vu); // Reversed // Even though all triangles might be initially valid, // a triangle may degenerate into a segment after applying // space transformation. if (dSafeNormalize3(Contact->normal)) { // No sense to save on single type conversion in algorithm of this size. // If there would be a custom typedef for distance type it could be used // instead of dReal. However using float directly is the loss of abstraction // and possible loss of precision in future. /*float*/ dReal T = Faces[i].mDistance; Contact->pos[0] = Origin[0] + (Direction[0] * T); Contact->pos[1] = Origin[1] + (Direction[1] * T); Contact->pos[2] = Origin[2] + (Direction[2] * T); Contact->pos[3] = REAL(0.0); Contact->depth = T; Contact->g1 = TriMesh; Contact->g2 = RayGeom; Contact->side1 = TriIndex; Contact->side2 = -1; OutTriCount++; // Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue" if (OutTriCount >= (Flags & NUMC_MASK)) { break; } } } } return OutTriCount; }
int dCollideRTL(dxGeom* g1, dxGeom* RayGeom, int Flags, dContactGeom* Contacts, int Stride){ dxTriMesh* TriMesh = (dxTriMesh*)g1; const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); RayCollider& Collider = TriMesh->_RayCollider; dReal Length = dGeomRayGetLength(RayGeom); int FirstContact, BackfaceCull; dGeomRayGetParams(RayGeom, &FirstContact, &BackfaceCull); int ClosestHit = dGeomRayGetClosestHit(RayGeom); Collider.SetFirstContact(FirstContact != 0); Collider.SetClosestHit(ClosestHit != 0); Collider.SetCulling(BackfaceCull != 0); Collider.SetMaxDist(Length); dVector3 Origin, Direction; dGeomRayGet(RayGeom, Origin, Direction); /* Make Ray */ Ray WorldRay; WorldRay.mOrig.x = Origin[0]; WorldRay.mOrig.y = Origin[1]; WorldRay.mOrig.z = Origin[2]; WorldRay.mDir.x = Direction[0]; WorldRay.mDir.y = Direction[1]; WorldRay.mDir.z = Direction[2]; /* Intersect */ Matrix4x4 amatrix; int TriCount = 0; if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix))) { TriCount = TriMesh->Faces.GetNbFaces(); } if (TriCount == 0) { return 0; } const CollisionFace* Faces = TriMesh->Faces.GetFaces(); int OutTriCount = 0; for (int i = 0; i < TriCount; i++) { if (OutTriCount == (Flags & 0xffff)) { break; } if (TriMesh->RayCallback == null || TriMesh->RayCallback(TriMesh, RayGeom, Faces[i].mFaceID, Faces[i].mU, Faces[i].mV)) { const int& TriIndex = Faces[i].mFaceID; if (!Callback(TriMesh, RayGeom, TriIndex)) { continue; } dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); dVector3 dv[3]; FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); float T = Faces[i].mDistance; Contact->pos[0] = Origin[0] + (Direction[0] * T); Contact->pos[1] = Origin[1] + (Direction[1] * T); Contact->pos[2] = Origin[2] + (Direction[2] * T); Contact->pos[3] = REAL(0.0); dVector3 vu; vu[0] = dv[1][0] - dv[0][0]; vu[1] = dv[1][1] - dv[0][1]; vu[2] = dv[1][2] - dv[0][2]; vu[3] = REAL(0.0); dVector3 vv; vv[0] = dv[2][0] - dv[0][0]; vv[1] = dv[2][1] - dv[0][1]; vv[2] = dv[2][2] - dv[0][2]; vv[3] = REAL(0.0); dCROSS(Contact->normal, =, vv, vu); // Reversed dNormalize3(Contact->normal); Contact->depth = T; Contact->g1 = TriMesh; Contact->g2 = RayGeom; OutTriCount++; } } return OutTriCount; }