void PhysicsPlaneGeom::changed(ConstFieldMaskArg whichField, UInt32 origin, BitVector details) { Inherited::changed(whichField, origin, details); if(whichField & ParametersFieldMask) { dGeomPlaneSetParams(_GeomID, getParameters().x(), getParameters().y(), getParameters().z(), getParameters().w()); } }
IoObject *IoODEPlane_setParams(IoODEPlane *self, IoObject *locals, IoMessage *m) { dReal a = IoMessage_locals_doubleArgAt_(m, locals, 0); dReal b = IoMessage_locals_doubleArgAt_(m, locals, 1); dReal c = IoMessage_locals_doubleArgAt_(m, locals, 2); dReal d = IoMessage_locals_doubleArgAt_(m, locals, 3); IoODEPlane_assertHasPlaneId(self, locals, m); dGeomPlaneSetParams(GEOMID, a, b, c, d); return self; }
//============================================================================== void OdePlane::updateEngineData() { const Eigen::Isometry3d& tf = mParentCollisionObject->getTransform(); const Eigen::Vector3d pos = tf.translation(); const Eigen::Matrix3d rot = tf.linear(); auto plane = static_cast<const dynamics::PlaneShape*>( mParentCollisionObject->getShape().get()); const Eigen::Vector3d& normal = plane->getNormal(); const double offset = plane->getOffset(); const Eigen::Vector3d& normal2 = rot*normal; const double offset2 = offset + pos.dot(normal2); dGeomPlaneSetParams(mGeomId, normal2.x(), normal2.y(), normal2.z(), offset2); }
int test_plane_point_depth() { int j; dVector3 n,p,q,a,b; // n = plane normal dReal d; dSimpleSpace space(0); dGeomID plane = dCreatePlane (0,0,0,1,0); dSpaceAdd (space,plane); // ********** make a random plane for (j=0; j<3; j++) n[j] = dRandReal() - 0.5; dNormalize3 (n); d = dRandReal() - 0.5; dGeomPlaneSetParams (plane,n[0],n[1],n[2],d); dPlaneSpace (n,p,q); // ********** test point on plane has depth 0 a[0] = dRandReal() - 0.5; a[1] = dRandReal() - 0.5; a[2] = 0; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED(); // ********** test arbitrary depth point a[0] = dRandReal() - 0.5; a[1] = dRandReal() - 0.5; a[2] = dRandReal() - 0.5; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol) FAILED(); // ********** test depth-1 point a[0] = dRandReal() - 0.5; a[1] = dRandReal() - 0.5; a[2] = -1; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED(); PASSED(); }
void PhysicsPlaneGeom::setParams(const Vec4f &value ) { PhysicsPlaneGeomPtr tmpPtr(*this); dGeomPlaneSetParams(tmpPtr->id, value.x(), value.y(), value.z(), value.w()); PhysicsPlaneGeomBase::setParams(value); }
int test_ray_and_plane() { int j; dContactGeom contact; dVector3 n,p,q,a,b,g,h; // n,d = plane parameters dMatrix3 R; dReal d; dSimpleSpace space(0); dGeomID ray = dCreateRay (0,0); dGeomID plane = dCreatePlane (0,0,0,1,0); dSpaceAdd (space,ray); dSpaceAdd (space,plane); // ********** make a random plane for (j=0; j<3; j++) n[j] = dRandReal() - 0.5; dNormalize3 (n); d = dRandReal() - 0.5; dGeomPlaneSetParams (plane,n[0],n[1],n[2],d); dPlaneSpace (n,p,q); // ********** test finite length ray below plane dGeomRaySetLength (ray,0.09); a[0] = dRandReal()-0.5; a[1] = dRandReal()-0.5; a[2] = -dRandReal()*0.5 - 0.1; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; dGeomSetPosition (ray,b[0],b[1],b[2]); dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1, dRandReal()*2-1,dRandReal()*10-5); dGeomSetRotation (ray,R); if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); // ********** test finite length ray above plane a[0] = dRandReal()-0.5; a[1] = dRandReal()-0.5; a[2] = dRandReal()*0.5 + 0.01; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; g[0] = dRandReal()-0.5; g[1] = dRandReal()-0.5; g[2] = dRandReal() + 0.01; for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j]; dNormalize3 (h); dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); dGeomRaySetLength (ray,10); if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); // ********** test finite length ray that intersects plane a[0] = dRandReal()-0.5; a[1] = dRandReal()-0.5; a[2] = dRandReal()-0.5; for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j]; g[0] = dRandReal()-0.5; g[1] = dRandReal()-0.5; g[2] = dRandReal()-0.5; for (j=0; j<3; j++) h[j] = g[0]*p[j] + g[1]*q[j] + g[2]*n[j]; dNormalize3 (h); dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); dGeomRaySetLength (ray,10); if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom))) { // test that contact is on plane surface if (dFabs (dCalcVectorDot3(contact.pos,n) - d) > tol) FAILED(); // also check normal signs if (dCalcVectorDot3 (h,contact.normal) > 0) FAILED(); // also check contact point depth if (dFabs (dGeomPlanePointDepth (plane,contact.pos[0],contact.pos[1],contact.pos[2])) > tol) FAILED(); draw_all_objects (space); } // ********** test ray that just misses for (j=0; j<3; j++) b[j] = (1+d)*n[j]; for (j=0; j<3; j++) h[j] = -n[j]; dGeomRaySet (ray,b[0],b[1],b[2],h[0],h[1],h[2]); dGeomRaySetLength (ray,0.99); if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 0) FAILED(); // ********** test ray that just hits dGeomRaySetLength (ray,1.01); if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); // ********** test polarity with typical ground plane dGeomPlaneSetParams (plane,0,0,1,0); for (j=0; j<3; j++) a[j] = 0.1; for (j=0; j<3; j++) b[j] = 0; a[2] = 1; b[2] = -1; dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]); dGeomRaySetLength (ray,2); if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); if (dFabs (contact.depth - 1) > tol) FAILED(); a[2] = -1; b[2] = 1; dGeomRaySet (ray,a[0],a[1],a[2],b[0],b[1],b[2]); if (dCollide (ray,plane,1,&contact,sizeof(dContactGeom)) != 1) FAILED(); if (dFabs (contact.depth - 1) > tol) FAILED(); PASSED(); }