/*
* This takes what is supposed to be a rotation matrix,
* and make sure it is correct.
* Note: this operates on rows, not columns, because for rotations
* both ways give equivalent results.
*/
void dOrthogonalizeR(dMatrix3 m)
{
dReal n0 = dLENGTHSQUARED(m);
if (n0 != 1)
dSafeNormalize3(m);
// project row[0] on row[1], should be zero
dReal proj = dDOT(m, m+4);
if (proj != 0) {
// Gram-Schmidt step on row[1]
m[4] -= proj * m[0];
m[5] -= proj * m[1];
m[6] -= proj * m[2];
}
dReal n1 = dLENGTHSQUARED(m+4);
if (n1 != 1)
dSafeNormalize3(m+4);
/* just overwrite row[2], this makes sure the matrix is not
a reflection */
dCROSS(m+8, =, m, m+4);
m[3] = m[4+3] = m[8+3] = 0;
}
/*
* This takes what is supposed to be a rotation matrix,
* and make sure it is correct.
* Note: this operates on rows, not columns, because for rotations
* both ways give equivalent results.
*/
void dOrthogonalizeR(dMatrix3 m)
{
dReal n0 = dCalcVectorLengthSquare3(m);
if (n0 != 1)
dSafeNormalize3(m);
// project row[0] on row[1], should be zero
dReal proj = dCalcVectorDot3(m, m+4);
if (proj != 0) {
// Gram-Schmidt step on row[1]
m[4] -= proj * m[0];
m[5] -= proj * m[1];
m[6] -= proj * m[2];
}
dReal n1 = dCalcVectorLengthSquare3(m+4);
if (n1 != 1)
dSafeNormalize3(m+4);
/* just overwrite row[2], this makes sure the matrix is not
a reflection */
dCalcVectorCross3(m+8, m, m+4);
m[3] = m[4+3] = m[8+3] = 0;
}
int
webots_physics_collide (dGeomID g1, dGeomID g2)
{
//check the wheel friction between the wheels and the field plane
//hence check if g1 or g2 is field first,
//then check if other object is a wheel
//get the robot id and the wheel id of this wheel
dGeomID field_geom = NULL;
dGeomID wheel_geom = NULL;
if (g1 == ground_geom)
{
field_geom = g1;
wheel_geom = g2;//possible wheel geom
}
else if (g2 == ground_geom)
{
field_geom = g2;
wheel_geom = g1;//possible wheel geom
}
else
//collision is not handled here since it is not a field-wheel collision (no field collision)
return 0;
//now check if the wheel_geom is really a wheel geom
bool wheel_found = false;
uint8 team_id = BLUE_TEAM;
uint8 player_id = 0;
uint8 wheel_id = 0;
//blue team
for (uint8 i = 0; i < blue_team.size () && !wheel_found; i++)
{
for (uint8 j = 0; j < blue_team[i].getNWheels (); j++)
{
if (blue_team[i].robot_wheels_geom[j] == wheel_geom)
{
wheel_found = true;
player_id = i;
wheel_id = j;
break;
}
}
}
//yellow team
for (uint8 i = 0; i < yellow_team.size () && !wheel_found; i++)
{
for (uint8 j = 0; j < yellow_team[i].getNWheels (); j++)
{
if (yellow_team[i].robot_wheels_geom[j] == wheel_geom)
{
wheel_found = true;
team_id = YELLOW_TEAM;
player_id = i;
wheel_id = j;
break;
}
}
}
if (!wheel_found)//collision is not handled here since it is not a field-wheel collision (now wheel collision)
return 0;
char buffer[80];
std::vector<CollisionRobot>* team;
if (team_id == BLUE_TEAM)
team = &blue_team;
else
team = &yellow_team;
double wheel_axial_orientation = (*team)[player_id].getWheelAxisOrientation (wheel_id) + M_PI_2;
// sprintf (buffer, "wheel_id: %d\t axial_rot: %f\n ", wheel_id, wheel_axial_orientation);
// see how many collision points there are between these objects
n_contacts = dCollide (g1, g2, MAX_CONTACTS, &contacts[0].geom, sizeof(dContact));
uint8 contact_id = 0;
for (; contact_id < n_contacts; contact_id++)
{
// custom parameters for creating the contact joint
// remove or tune these contact parameters to suit your needs
contacts[contact_id].surface.mode = dContactApprox1 | dContactMu2 | dContactFDir1 | dContactSoftCFM;
// contacts[contact_id].surface.mu = 0.05f;
// contacts[contact_id].surface.mu2 = 2.0f;
contacts[contact_id].surface.mu = 2.0f;
contacts[contact_id].surface.mu2 = 0.05f;
contacts[contact_id].surface.soft_cfm = 0.002;
contacts[contact_id].fdir1[0] = cos (wheel_axial_orientation);
contacts[contact_id].fdir1[1] = sin (wheel_axial_orientation);
contacts[contact_id].fdir1[2] = 0;
contacts[contact_id].fdir1[3] = 0;
dSafeNormalize3(contacts[contact_id].fdir1);//it is already normalized but anyway
// sprintf (buffer, "wheel_id: %d\t contact_id: %d\t fdir_x: %f\t fdir_y: %f\t fdir_z: %f\n ", wheel_id, contact_id,
// contacts[contact_id].fdir1[0], contacts[contact_id].fdir1[1], contacts[contact_id].fdir1[2]);
//
// dWebotsConsolePrintf(buffer);
// create a contact joint that will prevent the two bodies from intersecting
// note that contact joints are added to the contact_joint_group
contact_joints[contact_id] = dJointCreateContact (world, contact_joint_group, &contacts[contact_id]);
// attach joint between the body and the static environment (0)
dJointAttach (contact_joints[contact_id], (*team)[player_id].robot_wheels_body[wheel_id], 0);
}
return 1;// collision was handled above
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (SphereGeom->type == dSphereClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache();
SphereCollider& Collider = pccColliderCache->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
Sphere.mCenter.x = Position[0];
Sphere.mCenter.y = Position[1];
Sphere.mCenter.z = Position[2];
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & NUMC_MASK)){
break;
}
const int TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
vu[0] = v1[0] - v0[0];
vu[1] = v1[1] - v0[1];
vu[2] = v1[2] - v0[2];
vu[3] = REAL(0.0);
dVector3 vv;
vv[0] = v2[0] - v0[0];
vv[1] = v2[1] - v0[1];
vv[2] = v2[2] - v0[2];
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCROSS(Plane, =, vu, vv);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(Plane)) {
continue;
}
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
dReal side = dDOT(Plane,Position) - dDOT(Plane, v0);
if(side < REAL(0.0)) {
continue;
}
dReal Depth;
dReal u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesn't hit triangle
}
if (Depth < REAL(0.0)){
continue; // Negative depth does not produce a contact
}
dVector3 ContactPos;
dReal w = REAL(1.0) - u - v;
ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dir[0] = Position[0]-ContactPos[0];
dir[1] = Position[1]-ContactPos[1];
dir[2] = Position[2]-ContactPos[2];
dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
// Since Depth already had a requirement to be non-negative,
// negative direction projections should not be allowed as well,
// as otherwise the multiplication will result in negative contact depth.
if (dirProj < REAL(0.0)){
continue; // Zero contact depth could be ignored
}
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
Contact->pos[0] = ContactPos[0];
Contact->pos[1] = ContactPos[1];
Contact->pos[2] = ContactPos[2];
Contact->pos[3] = REAL(0.0);
// Using normal as plane (reversed)
Contact->normal[0] = -Plane[0];
Contact->normal[1] = -Plane[1];
Contact->normal[2] = -Plane[2];
Contact->normal[3] = REAL(0.0);
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
#if !defined MERGECONTACTS // Merge all contacts into 1
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
#endif // Otherwise assigned later
Contact->side1 = TriIndex;
OutTriCount++;
}
#if defined MERGECONTACTS // Merge all contacts into 1
if (OutTriCount > 0){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){
dVector3 pos;
pos[0] = Contact->pos[0];
pos[1] = Contact->pos[1];
pos[2] = Contact->pos[2];
dVector3 normal;
normal[0] = Contact->normal[0] * Contact->depth;
normal[1] = Contact->normal[1] * Contact->depth;
normal[2] = Contact->normal[2] * Contact->depth;
int TriIndex = Contact->side1;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
pos[0] += TempContact->pos[0];
pos[1] += TempContact->pos[1];
pos[2] += TempContact->pos[2];
normal[0] += TempContact->normal[0] * TempContact->depth;
normal[1] += TempContact->normal[1] * TempContact->depth;
normal[2] += TempContact->normal[2] * TempContact->depth;
TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
}
Contact->side1 = TriIndex;
Contact->pos[0] = pos[0] / OutTriCount;
Contact->pos[1] = pos[1] / OutTriCount;
Contact->pos[2] = pos[2] / OutTriCount;
// Remember to divide in square space.
Contact->depth = dSqrt(dDOT(normal, normal) / OutTriCount);
if (Contact->depth > dEpsilon) { // otherwise the normal is too small
dVector3Copy(Contact->normal, normal);
dNormalize3(Contact->normal);
} // otherwise original Contact's normal would be used and it should be already normalized
}
return 1;
}
else return 0;
#elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts
if (OutTriCount != 0){
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
dVector3 Normal;
dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
Normal[0] = FirstContact->normal[0] * FirstContact->depth;
Normal[1] = FirstContact->normal[1] * FirstContact->depth;
Normal[2] = FirstContact->normal[2] * FirstContact->depth;
Normal[3] = FirstContact->normal[3] * FirstContact->depth;
for (int i = 1; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Normal[0] += Contact->normal[0] * Contact->depth;
Normal[1] += Contact->normal[1] * Contact->depth;
Normal[2] += Contact->normal[2] * Contact->depth;
Normal[3] += Contact->normal[3] * Contact->depth;
}
dNormalize3(Normal);
for (int i = 0; i < OutTriCount; i++){
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
Contact->normal[0] = Normal[0];
Contact->normal[1] = Normal[1];
Contact->normal[2] = Normal[2];
Contact->normal[3] = Normal[3];
}
}
return OutTriCount;
}
else return 0;
#else // none of MERGECONTACTS and MERGECONTACTNORMALS // Just return
return OutTriCount;
#endif // MERGECONTACTS
}
else return 0;
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
dIASSERT (Stride >= (int)sizeof(dContactGeom));
dIASSERT (g1->type == dTriMeshClass);
dIASSERT (SphereGeom->type == dSphereClass);
dIASSERT ((Flags & NUMC_MASK) >= 1);
dxTriMesh* TriMesh = (dxTriMesh*)g1;
// Init
const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);
const unsigned uiTLSKind = TriMesh->getParentSpaceTLSKind();
dIASSERT(uiTLSKind == SphereGeom->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
SphereCollider& Collider = pccColliderCache->_SphereCollider;
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
// Sphere
Sphere Sphere;
dCopyVector3(Sphere.mCenter, Position);
Sphere.mRadius = Radius;
Matrix4x4 amatrix;
// TC results
if (TriMesh->doSphereTC) {
dxTriMesh::SphereTC* sphereTC = 0;
for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
sphereTC = &TriMesh->SphereTCCache[i];
break;
}
}
if (!sphereTC){
TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());
sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
sphereTC->Geom = SphereGeom;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null,
&MakeMatrix(TLPosition, TLRotation, amatrix));
}
if (! Collider.GetContactStatus()) {
// no collision occurred
return 0;
}
// get results
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0){
if (TriMesh->ArrayCallback != null){
TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
}
int OutTriCount = 0;
for (int i = 0; i < TriCount; i++){
if (OutTriCount == (Flags & NUMC_MASK)){
break;
}
const int TriIndex = Triangles[i];
dVector3 dv[3];
if (!Callback(TriMesh, SphereGeom, TriIndex))
continue;
FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);
dVector3& v0 = dv[0];
dVector3& v1 = dv[1];
dVector3& v2 = dv[2];
dVector3 vu;
dSubtractVectors3r4(vu, v1, v0);
vu[3] = REAL(0.0);
dVector3 vv;
dSubtractVectors3r4(vv, v2, v0);
vv[3] = REAL(0.0);
// Get plane coefficients
dVector4 Plane;
dCalcVectorCross3(Plane, vu, vv);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(Plane)) {
continue;
}
/* If the center of the sphere is within the positive halfspace of the
* triangle's plane, allow a contact to be generated.
* If the center of the sphere made it into the positive halfspace of a
* back-facing triangle, then the physics update and/or velocity needs
* to be adjusted (penetration has occured anyway).
*/
dReal side = dCalcVectorDot3(Plane, Position) - dCalcVectorDot3(Plane, v0);
if(side < REAL(0.0))
{
continue;
}
dReal Depth;
dReal u, v;
if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
continue; // Sphere doesn't hit triangle
}
if (Depth < REAL(0.0)){
continue; // Negative depth does not produce a contact
}
dVector3 ContactPos;
dReal w = REAL(1.0) - u - v;
dAddScaledVectors3r4(ContactPos, v1, v2, u, v);
dAddScaledVector3r4(ContactPos, v0, w);
// Depth returned from GetContactData is depth along
// contact point - sphere center direction
// we'll project it to contact normal
dVector3 dir;
dSubtractVectors3r4(dir, Position, ContactPos);
dReal dirProj = dCalcVectorDot3(dir, Plane) / dCalcVectorLength3(dir);
// Since Depth already had a requirement to be non-negative,
// negative direction projections should not be allowed as well,
// as otherwise the multiplication will result in negative contact depth.
if (dirProj < REAL(0.0))
continue; // Zero contact depth could be ignored
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);
dCopyVector3r4(Contact->pos, ContactPos);
// Using normal as plane (reversed)
dCopyNegatedVector3r4(Contact->normal, Plane);
Contact->depth = Depth * dirProj;
//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
// We need to set these unconditionally, as the merging may fail! - Bram
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
Contact->side1 = TriIndex;
OutTriCount++;
}
if (OutTriCount > 0)
{
if (TriMesh->SphereContactsMergeOption == MERGE_CONTACTS_FULLY)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
Contact->g1 = TriMesh;
Contact->g2 = SphereGeom;
Contact->side2 = -1;
if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 pos;
dCopyVector3r4(pos, Contact->pos);
dVector3 normal;
dCopyScaledVector3r4(normal, Contact->normal, Contact->depth);
int TriIndex = Contact->side1;
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddVector3r4(pos, TempContact->pos);
dAddScaledVector3r4(normal, TempContact->normal, TempContact->depth);
TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
}
Contact->side1 = TriIndex;
dReal invOutTriCount = dRecip(OutTriCount);
dCopyScaledVector3r4(Contact->pos, pos, invOutTriCount);
if ( !dSafeNormalize3(normal) )
return OutTriCount; // Cannot merge in this pathological case
// Using a merged normal, means that for each intersection, this new normal will be less effective in solving the intersection.
// That is why we need to correct this by increasing the depth for each intersection.
// The maximum of the adjusted depths is our newly merged depth value - Bram.
dReal mergedDepth = REAL(0.0);
dReal minEffectiveness = REAL(0.5);
for ( int i = 0; i < OutTriCount; ++i )
{
dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
dReal effectiveness = dCalcVectorDot3(normal, TempContact->normal);
if ( effectiveness < dEpsilon )
return OutTriCount; // Cannot merge this pathological case
// Cap our adjustment for the new normal to a factor 2, meaning a 60 deg change in normal.
effectiveness = ( effectiveness < minEffectiveness ) ? minEffectiveness : effectiveness;
dReal adjusted = TempContact->depth / effectiveness;
mergedDepth = ( mergedDepth < adjusted ) ? adjusted : mergedDepth;
}
Contact->depth = mergedDepth;
dCopyVector3r4(Contact->normal, normal);
}
return 1;
}
else if (TriMesh->SphereContactsMergeOption == MERGE_CONTACT_NORMALS)
{
if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT))
{
dVector3 Normal;
dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
dCopyScaledVector3r4(Normal, FirstContact->normal, FirstContact->depth);
for (int i = 1; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dAddScaledVector3r4(Normal, Contact->normal, Contact->depth);
}
dNormalize3(Normal);
for (int i = 0; i < OutTriCount; i++)
{
dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);
dCopyVector3r4(Contact->normal, Normal);
}
}
return OutTriCount;
}
else
{
dIASSERT(TriMesh->SphereContactsMergeOption == DONT_MERGE_CONTACTS);
return OutTriCount;
}
}
else return 0;
}
else return 0;
}
// test one mesh triangle on intersection with capsule
void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule(
const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
uint8 flags)
{
// calculate edges
SUBTRACT(v1,v0,m_vE0);
SUBTRACT(v2,v1,m_vE1);
SUBTRACT(v0,v2,m_vE2);
dVector3 _minus_vE0;
SUBTRACT(v0,v1,_minus_vE0);
// calculate poly normal
dCalcVectorCross3(m_vN,m_vE1,_minus_vE0);
// Even though all triangles might be initially valid,
// a triangle may degenerate into a segment after applying
// space transformation.
if (!dSafeNormalize3(m_vN))
{
return;
}
// create plane from triangle
dReal plDistance = -dCalcVectorDot3(v0,m_vN);
dVector4 plTrianglePlane;
CONSTRUCTPLANE(plTrianglePlane,m_vN,plDistance);
// calculate capsule distance to plane
dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,m_vCapsulePosition);
// Capsule must be over positive side of triangle
if (fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/)
{
// if not don't generate contacts
return;
}
dVector3 vPnt0;
SET (vPnt0,v0);
dVector3 vPnt1;
SET (vPnt1,v1);
dVector3 vPnt2;
SET (vPnt2,v2);
if (fDistanceCapsuleCenterToPlane < 0 )
{
SET (vPnt0,v0);
SET (vPnt1,v2);
SET (vPnt2,v1);
}
// do intersection test and find best separating axis
if (!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags))
{
// if not found do nothing
return;
}
// if best separation axis is not found
if (m_iBestAxis == 0 )
{
// this should not happen (we should already exit in that case)
dIASSERT(FALSE);
// do nothing
return;
}
// calculate caps centers in absolute space
dVector3 vCposTrans;
vCposTrans[0] = m_vCapsulePosition[0] + m_vNormal[0]*m_vCapsuleRadius;
vCposTrans[1] = m_vCapsulePosition[1] + m_vNormal[1]*m_vCapsuleRadius;
vCposTrans[2] = m_vCapsulePosition[2] + m_vNormal[2]*m_vCapsuleRadius;
dVector3 vCEdgePoint0;
vCEdgePoint0[0] = vCposTrans[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[1] = vCposTrans[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint0[2] = vCposTrans[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
dVector3 vCEdgePoint1;
vCEdgePoint1[0] = vCposTrans[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[1] = vCposTrans[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
vCEdgePoint1[2] = vCposTrans[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius);
// transform capsule edge points into triangle space
vCEdgePoint0[0] -= vPnt0[0];
vCEdgePoint0[1] -= vPnt0[1];
vCEdgePoint0[2] -= vPnt0[2];
vCEdgePoint1[0] -= vPnt0[0];
vCEdgePoint1[1] -= vPnt0[1];
vCEdgePoint1[2] -= vPnt0[2];
dVector4 plPlane;
dVector3 _minus_vN;
_minus_vN[0] = -m_vN[0];
_minus_vN[1] = -m_vN[1];
_minus_vN[2] = -m_vN[2];
// triangle plane
CONSTRUCTPLANE(plPlane,_minus_vN,0);
//plPlane = Plane4f( -m_vN, 0);
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
// plane with edge 0
dVector3 vTemp;
dCalcVectorCross3(vTemp,m_vN,m_vE0);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE1);
CONSTRUCTPLANE(plPlane, vTemp, -(dCalcVectorDot3(m_vE0,vTemp)-REAL(1e-5)));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane ))
{
return;
}
dCalcVectorCross3(vTemp,m_vN,m_vE2);
CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5));
if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) {
return;
}
// return capsule edge points into absolute space
vCEdgePoint0[0] += vPnt0[0];
vCEdgePoint0[1] += vPnt0[1];
vCEdgePoint0[2] += vPnt0[2];
vCEdgePoint1[0] += vPnt0[0];
vCEdgePoint1[1] += vPnt0[1];
vCEdgePoint1[2] += vPnt0[2];
// calculate depths for both contact points
SUBTRACT(vCEdgePoint0,m_vCapsulePosition,vTemp);
dReal fDepth0 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
SUBTRACT(vCEdgePoint1,m_vCapsulePosition,vTemp);
dReal fDepth1 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt);
// clamp depths to zero
if (fDepth0 < 0)
{
fDepth0 = 0.0f;
}
if (fDepth1 < 0 )
{
fDepth1 = 0.0f;
}
// Cached contacts's data
// contact 0
dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result
m_gLocalContacts[m_ctContacts].fDepth = fDepth0;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint0);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
if (m_ctContacts < (m_iFlags & NUMC_MASK)) {
// contact 1
m_gLocalContacts[m_ctContacts].fDepth = fDepth1;
SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal);
SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint1);
m_gLocalContacts[m_ctContacts].nFlags = 1;
m_ctContacts++;
}
}
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;
}
void nearCallback(void *, dGeomID a, dGeomID b)
{
const unsigned max_contacts = 8;
dContact contacts[max_contacts];
if (!dGeomGetBody(a) && !dGeomGetBody(b))
return; // don't handle static geom collisions
int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
//clog << "got " << n << " contacts" << endl;
/* Simple contact merging:
* If we have contacts that are too close with the same normal, keep only
* the one with maximum depth.
* The epsilon that defines what "too close" means can be a heuristic.
*/
int new_n = 0;
dReal epsilon = 1e-1; // default
/* If we know one of the geoms is a sphere, we can base the epsilon on the
* sphere's radius.
*/
dGeomID s = 0;
if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
(dGeomGetClass(b) == dSphereClass && (s = b))) {
epsilon = dGeomSphereGetRadius(s) * 0.3;
}
for (int i=0; i<n; ++i) {
// this block draws the contact points before merging, in red
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(1, 0, 0);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.008);
// let's offset the line a bit to avoid drawing overlap issues
float xyzf[3], hprf[3];
dsGetViewpoint(xyzf, hprf);
dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
dVector3 v;
dSubtractVectors3(v, contacts[i].geom.pos, xyz);
dVector3 c;
dCalcVectorCross3(c, v, contacts[i].geom.pos);
dSafeNormalize3(c);
dVector3 pos1;
dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
dVector3 pos2;
dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
dsDrawLine(pos1, pos2);
// end of contacts drawing code
int closest_point = i;
for (int j=0; j<new_n; ++j) {
dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
if (alignment > 0.99 // about 8 degrees of difference
&&
dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
// they are too close
closest_point = j;
//clog << "found close points: " << j << " and " << i << endl;
break;
}
}
if (closest_point != i) {
// we discard one of the points
if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
// the new point is deeper, copy it over closest_point
contacts[closest_point] = contacts[i];
} else
contacts[new_n++] = contacts[i]; // the point is preserved
}
//clog << "reduced from " << n << " to " << new_n << endl;
n = new_n;
for (int i=0; i<n; ++i) {
contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
contacts[i].surface.mu = 10;
contacts[i].surface.bounce = 0.2;
contacts[i].surface.bounce_vel = 0;
contacts[i].surface.soft_erp = 1e-3;
//clog << "depth: " << contacts[i].geom.depth << endl;
dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(0, 0, 1);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.01);
dsSetColor(0, 1, 0);
dVector3 pos2;
dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
dsDrawLine(contacts[i].geom.pos, pos2);
}
//clog << "----" << endl;
}
