int dCollidePR(dxGeom* RayGeom, dxGeom* PlaneGeom, int Flags, dContactGeom* Contact, int Stride) { dVector3 Plane; dGeomPlaneGetParams(PlaneGeom, Plane); dVector3 Origin, Direction; dGeomRayGet(RayGeom, Origin, Direction); dReal Length = dGeomRayGetLength(RayGeom); dReal Denom = Plane[0] * Direction[0] + Plane[1] * Direction[1] + Plane[2] * Direction[2]; if (dFabs(Denom) < 0.00001f) { return 0; // Ray never hits } float T = -(Plane[3] + Plane[0] * Origin[0] + Plane[1] * Origin[1] + Plane[2] * Origin[2]) / Denom; if (T < 0 || T > Length) { return 0; // Ray hits but not within boundaries } Contact->pos[0] = Origin[0] + T * Direction[0]; Contact->pos[1] = Origin[1] + T * Direction[1]; Contact->pos[2] = Origin[2] + T * Direction[2]; Contact->pos[3] = REAL(0.0); //Contact->normal = 0; Contact->depth = 0.0f; Contact->g1 = RayGeom; Contact->g2 = PlaneGeom; return 1; }
void Test_ODEBodies::testPlane() { Core::resetCore(); ODE_SimulationAlgorithm *algorithm = new ODE_SimulationAlgorithm(); Physics::getPhysicsManager()->setPhysicalSimulationAlgorithm(algorithm); ODE_Plane *plane1 = new ODE_Plane("Plane1"); plane1->getParameter("Axis")->setValueFromString("(0.1, 0.0, 0.0)"); dynamic_cast<DoubleValue*>(plane1->getParameter("Distance"))->set(1.5); CollisionObject *co = plane1->getCollisionObjects().at(0); QVERIFY(plane1->getRigidBodyID() == 0); algorithm->resetPhysics(); QVERIFY(plane1->getRigidBodyID() == 0); QVERIFY(co->getNativeCollisionObject() == 0); QVERIFY(plane1->createODECollisionObjects()); QVERIFY(co->getNativeCollisionObject() != 0); dGeomID planeGeom = (dGeomID) co->getNativeCollisionObject(); //TODO check cast dVector4 params; dGeomPlaneGetParams(planeGeom, params); QCOMPARE((double) params[0], 1.0); QCOMPARE((double) params[1], 0.0); QCOMPARE((double) params[2], 0.0); QCOMPARE((double) params[3], 1.5); delete plane1; }
IoObject *IoODEPlane_params(IoODEPlane *self, IoObject *locals, IoMessage *m) { dVector4 params; IoODEPlane_assertHasPlaneId(self, locals, m); dGeomPlaneGetParams(GEOMID, params); return IoSeq_newWithODEVector4(IOSTATE, params); }
/***************************************************************************\ * Field Get * \***************************************************************************/ Vec4f PhysicsPlaneGeom::getParams(void) { PhysicsPlaneGeomPtr tmpPtr(*this); dVector4 t; dGeomPlaneGetParams(tmpPtr->id, t); return Vec4f(t[0], t[1],t[2], t[3]); }
static void printPlane (PrintingContext &c, dxGeom *g) { dVector4 e; dGeomPlaneGetParams (g,e); c.print ("type","plane"); c.print ("normal",e); c.print ("d",e[3]); }
static void draw_phys_plane(dGeomID geom) { dVector4 v; dGeomPlaneGetParams(geom, v); opengl_draw_grd((float) v[0], (float) v[1], (float) v[2], (float) v[3]); }
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (Cylinder->type == dCylinderClass); dIASSERT (Plane->type == dPlaneClass); dIASSERT ((flags & NUMC_MASK) >= 1); int GeomCount = 0; // count of used contactgeoms #ifdef dSINGLE const dReal toleranz = REAL(0.0001); #endif #ifdef dDOUBLE const dReal toleranz = REAL(0.0000001); #endif // Get the properties of the cylinder (length+radius) dReal radius, length; dGeomCylinderGetParams(Cylinder, &radius, &length); dVector3 &cylpos = Cylinder->final_posr->pos; // and the plane dVector4 planevec; dGeomPlaneGetParams(Plane, planevec); dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]}; dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]}; dVector3 G1Pos1, G1Pos2, vDir1; vDir1[0] = Cylinder->final_posr->R[2]; vDir1[1] = Cylinder->final_posr->R[6]; vDir1[2] = Cylinder->final_posr->R[10]; dReal s; s = length * REAL(0.5); G1Pos2[0] = vDir1[0] * s + cylpos[0]; G1Pos2[1] = vDir1[1] * s + cylpos[1]; G1Pos2[2] = vDir1[2] * s + cylpos[2]; G1Pos1[0] = vDir1[0] * -s + cylpos[0]; G1Pos1[1] = vDir1[1] * -s + cylpos[1]; G1Pos1[2] = vDir1[2] * -s + cylpos[2]; dVector3 C; // parallel-check s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2]; if(s < 0) s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel else s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel if(s < toleranz && s > (-toleranz)) { // discs are parallel to the plane // 1.compute if, and where contacts are dVector3 P; s = planevec[3] - dVector3Dot(planevec, G1Pos1); dReal t; t = planevec[3] - dVector3Dot(planevec, G1Pos2); if(s >= t) // s == t does never happen, { if(s >= 0) { // 1. Disc dVector3Copy(G1Pos1, P); } else return GeomCount; // no contacts } else { if(t >= 0) { // 2. Disc dVector3Copy(G1Pos2, P); } else return GeomCount; // no contacts } // 2. generate a coordinate-system on the disc dVector3 V1, V2; if(vDir1[0] < toleranz && vDir1[0] > (-toleranz)) { // not x-axis V1[0] = vDir1[0] + REAL(1.0); // random value V1[1] = vDir1[1]; V1[2] = vDir1[2]; } else { // maybe x-axis V1[0] = vDir1[0]; V1[1] = vDir1[1] + REAL(1.0); // random value V1[2] = vDir1[2]; } // V1 is now another direction than vDir1 // Cross-product dVector3Cross(V1, vDir1, V2); // make unit V2 t = dVector3Length(V2); t = radius / t; dVector3Scale(V2, t); // cross again dVector3Cross(V2, vDir1, V1); // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' // V1 = first axis // V2 = second axis // 3. generate contactpoints // Potential contact 1 dVector3Add(P, V1, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 2 dVector3Subtract(P, V1, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 3 dVector3Add(P, V2, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 4 dVector3Subtract(P, V2, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } else { dReal t = dVector3Dot(PlaneNormal, vDir1); C[0] = vDir1[0] * t - PlaneNormal[0]; C[1] = vDir1[1] * t - PlaneNormal[1]; C[2] = vDir1[2] * t - PlaneNormal[2]; s = dVector3Length(C); // move C onto the circle s = radius / s; dVector3Scale(C, s); // deepest point of disc 1 dVector3Add(C, G1Pos1, contact->pos); // depth of the deepest point contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth >= 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // C is still computed // deepest point of disc 2 dVector3Add(C, G1Pos2, contact->pos); // depth of the deepest point contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth >= 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } return GeomCount; }
int dCollideTrimeshPlane( dxGeom *o1, dxGeom *o2, int flags, dContactGeom* contacts, int skip ) { dIASSERT( skip >= (int)sizeof( dContactGeom ) ); dIASSERT( o1->type == dTriMeshClass ); dIASSERT( o2->type == dPlaneClass ); dIASSERT ((flags & NUMC_MASK) >= 1); // Alias pointers to the plane and trimesh dxTriMesh* trimesh = (dxTriMesh*)( o1 ); dVector4 plane; dGeomPlaneGetParams(o2, plane); o1 -> recomputeAABB(); o2 -> recomputeAABB(); //Find collision GDYNAMIC_ARRAY collision_result; GIM_CREATE_TRIMESHPLANE_CONTACTS(collision_result); gim_trimesh_plane_collisionODE(&trimesh->m_collision_trimesh,plane,&collision_result); if(collision_result.m_size == 0 ) { GIM_DYNARRAY_DESTROY(collision_result); return 0; } unsigned int contactcount = collision_result.m_size; unsigned int contactmax = (unsigned int)(flags & NUMC_MASK); if (contactcount > contactmax) { contactcount = contactmax; } dContactGeom* pcontact; vec4f * planecontact_results = GIM_DYNARRAY_POINTER(vec4f,collision_result); for(unsigned int i = 0; i < contactcount; i++ ) { pcontact = SAFECONTACT(flags, contacts, i, skip); pcontact->pos[0] = (*planecontact_results)[0]; pcontact->pos[1] = (*planecontact_results)[1]; pcontact->pos[2] = (*planecontact_results)[2]; pcontact->pos[3] = REAL(1.0); pcontact->normal[0] = plane[0]; pcontact->normal[1] = plane[1]; pcontact->normal[2] = plane[2]; pcontact->normal[3] = 0; pcontact->depth = (*planecontact_results)[3]; pcontact->g1 = o1; pcontact->g2 = o2; planecontact_results++; } GIM_DYNARRAY_DESTROY(collision_result); return (int)contactcount; }
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT ((flags & 0xffff) >= 1); unsigned char* pContactData = (unsigned char*)contact; int GeomCount = 0; // count of used contactgeoms #ifdef dSINGLE const dReal toleranz = 0.0001f; #endif #ifdef dDOUBLE const dReal toleranz = 0.0000001; #endif // Get the properties of the cylinder (length+radius) dReal radius, length; dGeomCylinderGetParams(Cylinder, &radius, &length); dVector3 &cylpos = Cylinder->final_posr->pos; // and the plane dVector4 planevec; dGeomPlaneGetParams(Plane, planevec); dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]}; dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]}; dVector3 G1Pos1, G1Pos2, vDir1; vDir1[0] = Cylinder->final_posr->R[2]; vDir1[1] = Cylinder->final_posr->R[6]; vDir1[2] = Cylinder->final_posr->R[10]; dReal s; s = length * dReal(0.5); G1Pos2[0] = vDir1[0] * s + cylpos[0]; G1Pos2[1] = vDir1[1] * s + cylpos[1]; G1Pos2[2] = vDir1[2] * s + cylpos[2]; G1Pos1[0] = vDir1[0] * -s + cylpos[0]; G1Pos1[1] = vDir1[1] * -s + cylpos[1]; G1Pos1[2] = vDir1[2] * -s + cylpos[2]; dVector3 C; // parallel-check s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2]; if(s < 0) s += dReal(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel else s -= dReal(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel if(s < toleranz && s > (-toleranz)) { // discs are parallel to the plane // 1.compute if, and where contacts are dVector3 P; s = planevec[3] - planevec[0] * G1Pos1[0] - planevec[1] * G1Pos1[1] - planevec[2] * G1Pos1[2]; dReal t; t = planevec[3] - planevec[0] * G1Pos2[0] - planevec[1] * G1Pos2[1] - planevec[2] * G1Pos2[2]; if(s >= t) // s == t does never happen, { if(s >= 0) { // 1. Disc P[0] = G1Pos1[0]; P[1] = G1Pos1[1]; P[2] = G1Pos1[2]; } else return GeomCount; // no contacts } else { if(t >= 0) { // 2. Disc P[0] = G1Pos2[0]; P[1] = G1Pos2[1]; P[2] = G1Pos2[2]; } else return GeomCount; // no contacts } // 2. generate a coordinate-system on the disc dVector3 V1, V2; if(vDir1[0] < toleranz && vDir1[0] > (-toleranz)) { // not x-axis V1[0] = vDir1[0] + dReal(1.0); // random value V1[1] = vDir1[1]; V1[2] = vDir1[2]; } else { // maybe x-axis V1[0] = vDir1[0]; V1[1] = vDir1[1] + dReal(1.0); // random value V1[2] = vDir1[2]; } // V1 is now another direction than vDir1 // Cross-product V2[0] = V1[1] * vDir1[2] - V1[2] * vDir1[1]; V2[1] = V1[2] * vDir1[0] - V1[0] * vDir1[2]; V2[2] = V1[0] * vDir1[1] - V1[1] * vDir1[0]; // make unit V2 t = dReal(sqrt(V2[0] * V2[0] + V2[1] * V2[1] + V2[2] * V2[2])); t = radius / t; V2[0] *= t; V2[1] *= t; V2[2] *= t; // cross again V1[0] = V2[1] * vDir1[2] - V2[2] * vDir1[1]; V1[1] = V2[2] * vDir1[0] - V2[0] * vDir1[2]; V1[2] = V2[0] * vDir1[1] - V2[1] * vDir1[0]; // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' // V1 = first axis // V2 = second axis // 3. generate contactpoints // Potential contact 1 contact->pos[0] = P[0] + V1[0]; contact->pos[1] = P[1] + V1[1]; contact->pos[2] = P[2] + V1[2]; contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth > 0) { contact->normal[0] = PlaneNormal[0]; contact->normal[1] = PlaneNormal[1]; contact->normal[2] = PlaneNormal[2]; contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & 0x0ffff)) return GeomCount; // enough contactgeoms pContactData += skip; contact = (dContactGeom*)pContactData; } // Potential contact 2 contact->pos[0] = P[0] - V1[0]; contact->pos[1] = P[1] - V1[1]; contact->pos[2] = P[2] - V1[2]; contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth > 0) { contact->normal[0] = PlaneNormal[0]; contact->normal[1] = PlaneNormal[1]; contact->normal[2] = PlaneNormal[2]; contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & 0x0ffff)) return GeomCount; // enough contactgeoms pContactData += skip; contact = (dContactGeom*)pContactData; } // Potential contact 3 contact->pos[0] = P[0] + V2[0]; contact->pos[1] = P[1] + V2[1]; contact->pos[2] = P[2] + V2[2]; contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth > 0) { contact->normal[0] = PlaneNormal[0]; contact->normal[1] = PlaneNormal[1]; contact->normal[2] = PlaneNormal[2]; contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & 0x0ffff)) return GeomCount; // enough contactgeoms pContactData += skip; contact = (dContactGeom*)pContactData; } // Potential contact 4 contact->pos[0] = P[0] - V2[0]; contact->pos[1] = P[1] - V2[1]; contact->pos[2] = P[2] - V2[2]; contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth > 0) { contact->normal[0] = PlaneNormal[0]; contact->normal[1] = PlaneNormal[1]; contact->normal[2] = PlaneNormal[2]; contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & 0x0ffff)) return GeomCount; // enough contactgeoms pContactData += skip; contact = (dContactGeom*)pContactData; } } else { dReal t = -((-PlaneNormal[0]) * vDir1[0] + (-PlaneNormal[1]) * vDir1[1] + (-PlaneNormal[2]) * vDir1[2]); C[0] = vDir1[0] * t - PlaneNormal[0]; C[1] = vDir1[1] * t - PlaneNormal[1]; C[2] = vDir1[2] * t - PlaneNormal[2]; s = dReal(sqrt(C[0] * C[0] + C[1] * C[1] + C[2] * C[2])); // move C onto the circle s = radius / s; C[0] *= s; C[1] *= s; C[2] *= s; // deepest point of disc 1 contact->pos[0] = C[0] + G1Pos1[0]; contact->pos[1] = C[1] + G1Pos1[1]; contact->pos[2] = C[2] + G1Pos1[2]; // depth of the deepest point contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth >= 0) { contact->normal[0] = PlaneNormal[0]; contact->normal[1] = PlaneNormal[1]; contact->normal[2] = PlaneNormal[2]; contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & 0x0ffff)) return GeomCount; // enough contactgeoms pContactData += skip; contact = (dContactGeom*)pContactData; } // C is still computed // deepest point of disc 2 contact->pos[0] = C[0] + G1Pos2[0]; contact->pos[1] = C[1] + G1Pos2[1]; contact->pos[2] = C[2] + G1Pos2[2]; // depth of the deepest point contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth >= 0) { contact->normal[0] = PlaneNormal[0]; contact->normal[1] = PlaneNormal[1]; contact->normal[2] = PlaneNormal[2]; contact->g1 = Cylinder; contact->g2 = Plane; GeomCount++; if( GeomCount >= (flags & 0x0ffff)) return GeomCount; // enough contactgeoms pContactData += skip; contact = (dContactGeom*)pContactData; } } return GeomCount; }
void draw_all_objects (dSpaceID space) { int i, j; draw_all_objects_called = 1; if (!graphical_test) return; int n = dSpaceGetNumGeoms (space); // draw all contact points dsSetColor (0,1,1); dSpaceCollide (space,0,&nearCallback); // draw all rays for (i=0; i<n; i++) { dGeomID g = dSpaceGetGeom (space,i); if (dGeomGetClass (g) == dRayClass) { dsSetColor (1,1,1); dVector3 origin,dir; dGeomRayGet (g,origin,dir); origin[2] += Z_OFFSET; dReal length = dGeomRayGetLength (g); for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j]; dsDrawLine (origin,dir); dsSetColor (0,0,1); dsDrawSphere (origin,dGeomGetRotation(g),0.01); } } // draw all other objects for (i=0; i<n; i++) { dGeomID g = dSpaceGetGeom (space,i); dVector3 pos; if (dGeomGetClass (g) != dPlaneClass) { memcpy (pos,dGeomGetPosition(g),sizeof(pos)); pos[2] += Z_OFFSET; } switch (dGeomGetClass (g)) { case dSphereClass: { dsSetColorAlpha (1,0,0,0.8); dReal radius = dGeomSphereGetRadius (g); dsDrawSphere (pos,dGeomGetRotation(g),radius); break; } case dBoxClass: { dsSetColorAlpha (1,1,0,0.8); dVector3 sides; dGeomBoxGetLengths (g,sides); dsDrawBox (pos,dGeomGetRotation(g),sides); break; } case dCapsuleClass: { dsSetColorAlpha (0,1,0,0.8); dReal radius,length; dGeomCapsuleGetParams (g,&radius,&length); dsDrawCapsule (pos,dGeomGetRotation(g),length,radius); break; } case dCylinderClass: { dsSetColorAlpha (0,1,0,0.8); dReal radius,length; dGeomCylinderGetParams (g,&radius,&length); dsDrawCylinder (pos,dGeomGetRotation(g),length,radius); break; } case dPlaneClass: { dVector4 n; dMatrix3 R,sides; dVector3 pos2; dGeomPlaneGetParams (g,n); dRFromZAxis (R,n[0],n[1],n[2]); for (j=0; j<3; j++) pos[j] = n[j]*n[3]; pos[2] += Z_OFFSET; sides[0] = 2; sides[1] = 2; sides[2] = 0.001; dsSetColor (1,0,1); for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j]; dsDrawLine (pos,pos2); dsSetColorAlpha (1,0,1,0.8); dsDrawBox (pos,R,sides); break; } } } }