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;
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
//if (!pause) dWorldStepFast (world,0.05, 1);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
/*{
for (int i = 1; i < IndexCount; i++) {
dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
}
}*/
{const dReal* Pos = dGeomGetPosition(TriMesh);
const dReal* Rot = dGeomGetRotation(TriMesh);
{for (int i = 0; i < IndexCount / 3; i++){
const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
}}}
if (Ray){
dVector3 Origin, Direction;
dGeomRayGet(Ray, Origin, Direction);
dReal Length = dGeomRayGetLength(Ray);
dVector3 End;
End[0] = Origin[0] + (Direction[0] * Length);
End[1] = Origin[1] + (Direction[1] * Length);
End[2] = Origin[2] + (Direction[2] * Length);
End[3] = Origin[3] + (Direction[3] * Length);
dsDrawLine(Origin, End);
}
}
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;
}
static void draw_phys_ray(dGeomID geom)
{
dVector3 p;
dVector3 v;
dReal l = dGeomRayGetLength(geom);
dGeomRayGet(geom, p, v);
opengl_draw_vec((float) p[0],
(float) p[1],
(float) p[2],
(float) (p[0] + v[0] * l),
(float) (p[1] + v[1] * l),
(float) (p[2] + v[2] * l));
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
int color_changed = 0;
if (i==selected) {
dsSetColor (0,0.7,1);
color_changed = 1;
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
color_changed = 1;
}
for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
if (color_changed) dsSetColor (1,1,0);
}
{for (int i = 0; i < RayCount; i++){
dVector3 Origin, Direction;
dGeomRayGet(Rays[i], Origin, Direction);
dReal Length = dGeomRayGetLength(Rays[i]);
dVector3 End;
End[0] = Origin[0] + (Direction[0] * Length);
End[1] = Origin[1] + (Direction[1] * Length);
End[2] = Origin[2] + (Direction[2] * Length);
End[3] = Origin[3] + (Direction[3] * Length);
dsDrawLine(Origin, End);
}}
}
void EOSOdeCollisionObject::showDebugObject(Ogre::SceneNode* parentSceneNode, bool visible)
{
EOSOdeTrimeshDebugObject* debugObj = NULL;
switch(this->collisionGeomType)
{
case EOSOdeCollisionObject::BoxGeomType:
this->debugObject = new EOSOdeBoxDebugObject(this->size);
break;
case EOSOdeCollisionObject::SphereGeomType:
this->debugObject = new EOSOdeSphereDebugObject(this->size.x);
break;
case EOSOdeCollisionObject::TrimeshGeomType:
debugObj = new EOSOdeTrimeshDebugObject((this->indexCount / 3) * 6);
debugObj->beginDefinition();
for(unsigned int i = 0, j = 0; i < this->indexCount; i+=3, j+=6)
{
debugObj->setVertex(j, Ogre::Vector3((Ogre::Real)this->vertices[this->indices[i]][0], (Ogre::Real)this->vertices[this->indices[i]][1], (Ogre::Real)this->vertices[this->indices[i]][2]));
debugObj->setVertex(j+1, Ogre::Vector3((Ogre::Real)this->vertices[this->indices[i+1]][0],(Ogre::Real)this->vertices[this->indices[i+1]][1],(Ogre::Real)this->vertices[this->indices[i+1]][2]));
debugObj->setVertex(j+2, Ogre::Vector3((Ogre::Real)this->vertices[this->indices[i+1]][0],(Ogre::Real)this->vertices[this->indices[i+1]][1],(Ogre::Real)this->vertices[this->indices[i+1]][2]));
debugObj->setVertex(j+3, Ogre::Vector3((Ogre::Real)this->vertices[this->indices[i+2]][0],(Ogre::Real)this->vertices[this->indices[i+2]][1],(Ogre::Real)this->vertices[this->indices[i+2]][2]));
debugObj->setVertex(j+4, Ogre::Vector3((Ogre::Real)this->vertices[this->indices[i+2]][0],(Ogre::Real)this->vertices[this->indices[i+2]][1],(Ogre::Real)this->vertices[this->indices[i+2]][2]));
debugObj->setVertex(j+5, Ogre::Vector3((Ogre::Real)this->vertices[this->indices[i]][0], (Ogre::Real)this->vertices[this->indices[i]][1], (Ogre::Real)this->vertices[this->indices[i]][2]));
}
debugObj->endDefinition();
this->debugObject = debugObj;
break;
case EOSOdeCollisionObject::RayGeomType:
dVector3 start, direction;
dGeomRayGet(this->collisionGeometry, start, direction);
Ogre::Real length = dGeomRayGetLength(this->collisionGeometry);
this->debugObject = new EOSOdeRayDebugObject(Ogre::Vector3(start[0], start[1], start[2]), Ogre::Vector3(direction[0], direction[1], direction[2]), length);
break;
}
parentSceneNode->attachObject(this->debugObject);
}
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;
}
// copied from an OpenDE demo program
//todo:pass trimesh as argument to this function
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb, Tmesh tm)
{
int i;
if (!g) return;
if (dGeomIsSpace(g))
{
displaySpace((dSpaceID)g);
return;
}
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dTriMeshClass)
{
//dTriIndex* Indices = DISP.tmd[i].indices;
const dReal* Pos = dGeomGetPosition(g);
const dReal* Rot = dGeomGetRotation(g);
for (int ii = 0; ii < (tm.indexSize/3); ii++)
{
const dReal v[9] = { // explicit conversion from float to dReal
tm.vertices[tm.indices[ii * 3 + 0] * 3 + 0],
tm.vertices[tm.indices[ii * 3 + 0] * 3 + 1],
tm.vertices[tm.indices[ii * 3 + 0] * 3 + 2],
tm.vertices[tm.indices[ii * 3 + 1] * 3 + 0],
tm.vertices[tm.indices[ii * 3 + 1] * 3 + 1],
tm.vertices[tm.indices[ii * 3 + 1] * 3 + 2],
tm.vertices[tm.indices[ii * 3 + 2] * 3 + 0],
tm.vertices[tm.indices[ii * 3 + 2] * 3 + 1],
tm.vertices[tm.indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}
//std::cout<<"done once"<<std::endl;
}
else
if (type == dRayClass)
{
dVector3 Origin, Direction;
dGeomRayGet(g, Origin, Direction);
dReal Length = dGeomRayGetLength(g);
dVector3 End;
End[0] = Origin[0] + (Direction[0] * Length);
End[1] = Origin[1] + (Direction[1] * Length);
End[2] = Origin[2] + (Direction[2] * Length);
End[3] = Origin[3] + (Direction[3] * Length);
double *ori = new double[3];
double *end = new double[4];
ori[0]=Origin[0];
ori[1]=Origin[1];
ori[2]=Origin[2];
end[0]=End[0];
end[1]=End[1];
end[2]=End[2];
end[3]=End[3];
dsDrawLine(ori, end);
}
else
show_aabb = 0;
if (show_aabb)
{
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
int dCollideSR(dxGeom* RayGeom, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
dReal Radius = dGeomSphereGetRadius(SphereGeom);
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
dReal Length = dGeomRayGetLength(RayGeom);
dVector3 Diff;
Diff[0] = Origin[0] - Position[0];
Diff[1] = Origin[1] - Position[1];
Diff[2] = Origin[2] - Position[2];
Diff[3] = Origin[3] - Position[3];
Direction[0] *= Length;
Direction[1] *= Length;
Direction[2] *= Length;
Direction[3] *= Length;
dReal A = Length * Length;
dReal B = dDOT(Diff, Direction);
dReal C = dDOT(Diff, Diff) - (Radius * Radius);
dReal Discr = B * B - A * C;
if (Discr < REAL(0.0)){
return 0;
}
else if (Discr > REAL(0.0)){
dReal T[2];
dReal Root = dSqrt(Discr);
dReal InvA = REAL(1.0) / A;
T[0] = (-B - Root) * InvA;
T[1] = (-B + Root) * InvA;
if (T[0] >= REAL(0.0)){
dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
//Contact0->normal = 0;
Contact0->depth = 0.0f;
Contact0->g1 = RayGeom;
Contact0->g2 = SphereGeom;
dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact1->normal = 0;
Contact1->depth = 0.0f;
Contact1->g1 = RayGeom;
Contact1->g2 = SphereGeom;
return 2;
}
else if (T[1] >= REAL(0.0)){
dContactGeom* Contact = CONTACT(Flags, Contacts, 1, Stride);
Contact->pos[0] = Origin[0] + T[1] * Direction[0];
Contact->pos[1] = Origin[1] + T[1] * Direction[1];
Contact->pos[2] = Origin[2] + T[1] * Direction[2];
Contact->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact->normal = 0;
Contact->depth = 0.0f;
Contact->g1 = RayGeom;
Contact->g2 = SphereGeom;
return 1;
}
else return 0;
}
else{
dReal T;
T = -B / A;
if (T >= REAL(0.0)){
dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
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] = Origin[3] + T * Direction[3];
//Contact->normal = 0;
Contact->depth = 0.0f;
Contact->g1 = RayGeom;
Contact->g2 = SphereGeom;
return 1;
}
else return 0;
}
}
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;
}
}
}
}
int dCollideBR(dxGeom* RayGeom, dxGeom* BoxGeom, int Flags, dContactGeom* Contacts, int Stride){
const dVector3& Position = *(const dVector3*)dGeomGetPosition(BoxGeom);
const dMatrix3& Rotation = *(const dMatrix3*)dGeomGetRotation(BoxGeom);
dVector3 Extents;
dGeomBoxGetLengths(BoxGeom, Extents);
Extents[0] /= 2;
Extents[1] /= 2;
Extents[2] /= 2;
Extents[3] /= 2;
dVector3 Origin, Direction;
dGeomRayGet(RayGeom, Origin, Direction);
dReal Length = dGeomRayGetLength(RayGeom);
dVector3 Diff;
Diff[0] = Origin[0] - Position[0];
Diff[1] = Origin[1] - Position[1];
Diff[2] = Origin[2] - Position[2];
Diff[3] = Origin[3] - Position[3];
Direction[0] *= Length;
Direction[1] *= Length;
Direction[2] *= Length;
Direction[3] *= Length;
dVector3 Rot[3];
Decompose(Rotation, Rot);
dVector3 TransOrigin;
TransOrigin[0] = dDOT(Diff, Rot[0]);
TransOrigin[1] = dDOT(Diff, Rot[1]);
TransOrigin[2] = dDOT(Diff, Rot[2]);
TransOrigin[3] = REAL(0.0);
dVector3 TransDirection;
TransDirection[0] = dDOT(Direction, Rot[0]);
TransDirection[1] = dDOT(Direction, Rot[1]);
TransDirection[2] = dDOT(Direction, Rot[2]);
TransDirection[3] = REAL(0.0);
dReal T[2];
T[0] = 0.0f;
T[1] = dInfinity;
bool Intersect = FindIntersection(TransOrigin, TransDirection, Extents, T[0], T[1]);
if (Intersect){
if (T[0] > REAL(0.0)){
dContactGeom* Contact0 = CONTACT(Flags, Contacts, 0, Stride);
Contact0->pos[0] = Origin[0] + T[0] * Direction[0];
Contact0->pos[1] = Origin[1] + T[0] * Direction[1];
Contact0->pos[2] = Origin[2] + T[0] * Direction[2];
Contact0->pos[3] = Origin[3] + T[0] * Direction[3];
//Contact0->normal = 0;
Contact0->depth = 0.0f;
Contact0->g1 = RayGeom;
Contact0->g2 = BoxGeom;
dContactGeom* Contact1 = CONTACT(Flags, Contacts, 1, Stride);
Contact1->pos[0] = Origin[0] + T[1] * Direction[0];
Contact1->pos[1] = Origin[1] + T[1] * Direction[1];
Contact1->pos[2] = Origin[2] + T[1] * Direction[2];
Contact1->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact1->normal = 0;
Contact1->depth = 0.0f;
Contact1->g1 = RayGeom;
Contact1->g2 = BoxGeom;
return 2;
}
else{
dContactGeom* Contact = CONTACT(Flags, Contacts, 0, Stride);
Contact->pos[0] = Origin[0] + T[1] * Direction[0];
Contact->pos[1] = Origin[1] + T[1] * Direction[1];
Contact->pos[2] = Origin[2] + T[1] * Direction[2];
Contact->pos[3] = Origin[3] + T[1] * Direction[3];
//Contact->normal = 0;
Contact->depth = 0.0f;
Contact->g1 = RayGeom;
Contact->g2 = BoxGeom;
return 1;
}
}
else return 0;
}
