virtual void updateBegin(double time) { _manager->getWorld()->stepSimulation(time, 10); btDispatcher *dispatcher = _manager->getWorld()->getDispatcher(); unsigned int max = dispatcher->getNumManifolds(); for (unsigned int i = 0; i < max; ++i) { btPersistentManifold *contact = dispatcher->getManifoldByIndexInternal(i); const btCollisionObject *oa = static_cast<const btCollisionObject*>(contact->getBody0()); const btCollisionObject *ob = static_cast<const btCollisionObject*>(contact->getBody1()); Entity h1 = *(static_cast<Entity*>(oa->getUserPointer())); EntityData *e1 = h1.get(); auto c1 = e1->addComponent<Component::Collision>(); Entity h2 = *(static_cast<Entity*>(ob->getUserPointer())); EntityData *e2 = h2.get(); auto c2 = e2->addComponent<Component::Collision>(); c1->addCollision(h2); c2->addCollision(h1); } }
bool MeshShape::checkCollisionPeer(SphereShape *target) { float position[3] = {0, 0, 0}; target->object->transformPoint(position, position); object->unTransformPoint(position, position); float normal[3]; float contactpoint[3]; float r = target->getRadius(); if (checkSphereMeshCollision(position, r, mesh, normal, contactpoint)) { vectorScale(normal, -1); object->transformVector(normal, normal); object->transformPoint(contactpoint, contactpoint); addCollision(object, target->object, normal, contactpoint); // vectorAdd(contactnormal, normal); return true; } return false; }
bool CollisionList::collide(Object *obj1, Object *obj2) { vector *spot = NULL; ListIterator<BoundBox> boxIterator1 = *ListIterator<BoundBox>(obj1->objType->boundBoxes).SetFirst(); ListIterator<BoundBox> boxIterator2 = *ListIterator<BoundBox>(obj2->objType->boundBoxes).SetFirst(); ListIterator<BoundPlane> planeIterator1 = *ListIterator<BoundPlane>(obj1->objType->boundPlanes).SetFirst(); ListIterator<BoundPlane> planeIterator2 = *ListIterator<BoundPlane>(obj2->objType->boundPlanes).SetFirst(); ListIterator<BoundSphere> sphereIterator1 = *ListIterator<BoundSphere>(obj1->objType->boundSpheres).SetFirst(); ListIterator<BoundSphere> sphereIterator2 = *ListIterator<BoundSphere>(obj2->objType->boundSpheres).SetFirst(); while(!boxIterator1.IsLast()) { while(!boxIterator2.IsLast()) { spot = boxBoxCollision(boxIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot), boxIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot)); if(addCollision(obj1, obj2, spot)) return true; boxIterator2.Next(); } boxIterator2.SetFirst(); while(!planeIterator2.IsLast()) { spot = boxPlaneCollision(boxIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot), planeIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot)); if(addCollision(obj1, obj2, spot)) return true; planeIterator2.Next(); } planeIterator2.SetFirst(); while(!sphereIterator2.IsLast()) { spot = boxSphereCollision(boxIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot), sphereIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot)); if(addCollision(obj1, obj2, spot)) return true; sphereIterator2.Next(); } sphereIterator2.SetFirst(); boxIterator1.Next(); } boxIterator2.SetFirst(); planeIterator2.SetFirst(); sphereIterator2.SetFirst(); while(!planeIterator1.IsLast()) { while(!boxIterator2.IsLast()) { spot = boxPlaneCollision(boxIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot), planeIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot)); if(addCollision(obj1, obj2, spot)) return true; boxIterator2.Next(); } boxIterator2.SetFirst(); while(!planeIterator2.IsLast()) { spot = planePlaneCollision(planeIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot), planeIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot)); if(addCollision(obj1, obj2, spot)) return true; planeIterator2.Next(); } planeIterator2.SetFirst(); while(!sphereIterator2.IsLast()) { spot = planeSphereCollision(planeIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot), sphereIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot)); if(addCollision(obj1, obj2, spot)) return true; sphereIterator2.Next(); } sphereIterator2.SetFirst(); planeIterator1.Next(); } boxIterator2.SetFirst(); planeIterator2.SetFirst(); sphereIterator2.SetFirst(); while(!sphereIterator1.IsLast()) { while(!boxIterator2.IsLast()) { spot = boxSphereCollision(boxIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot), sphereIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot)); if(addCollision(obj1, obj2, spot)) return true; boxIterator2.Next(); } boxIterator2.SetFirst(); while(!planeIterator2.IsLast()) { spot = planeSphereCollision(planeIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot), sphereIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot)); if(addCollision(obj1, obj2, spot)) return true; planeIterator2.Next(); } planeIterator2.SetFirst(); while(!sphereIterator2.IsLast()) { spot = sphereSphereCollision(sphereIterator1.GetCurrent()->preprocess(obj1->getPosition(), obj1->scale, obj1->rot), sphereIterator2.GetCurrent()->preprocess(obj2->getPosition(), obj2->scale, obj2->rot)); if(addCollision(obj1, obj2, spot)) return true; sphereIterator2.Next(); } sphereIterator2.SetFirst(); sphereIterator1.Next(); } return false; }
vrpn_HapticBoolean TexturePlane::collisionDetect(gstPHANToM *PHANToM) { vrpn_HapticPosition phantomPos, lastPhantPos, intersectionPoint; double depth = 0; vrpn_HapticPosition SCP; vrpn_HapticVector SCPnormal; double deltaT; static double deltaDist; vrpn_HapticPosition diff; deltaT = PHANToM->getDeltaT(); if (fadeActive) incrementFade(deltaT); vrpn_HapticVector phantomForce = PHANToM->getReactionForce_WC(); PHANToM->getLastPosition_WC(lastPhantPos); PHANToM->getPosition_WC(phantomPos); diff = lastPhantPos - phantomPos; deltaDist = diff.distToOrigin(); if (dataCB){ dataCB(data_time, lastPhantPos, phantomForce, userdata); data_time += deltaT; } //if the plane node is not in effect if(inEffect == FALSE){ safety_ineffect = TRUE; return FALSE; } inContact = getStateForPHANToM(PHANToM); #ifdef VRPN_USE_GHOST_31 if(!_touchableByPHANToM || _resetPHANToMContacts) { #else // Ghost 4.0 (and the default case) if(!isTouchableByPHANToM() || _resetPHANToMContacts) { #endif _resetPHANToMContacts = FALSE; inContact = FALSE; (void) updateStateForPHANToM(PHANToM,inContact); //printf("in if incontact is false\n"); return inContact; } phantomPos = fromWorld(phantomPos); //project the current phantomPosition onto the plane to get the SCP // if we don't have the constant plane assumed by texture computing // functions if (!usingTexture || !usingAssumedTextureBasePlane(plane) || !dynamicParent){ depth = -(phantomPos[0]*plane.a() + phantomPos[1]*plane.b() + phantomPos[2]*plane.c() + plane.d()); SCP = plane.projectPoint(phantomPos); //set the SCPnormal to be the normal of the plane SCPnormal = plane.normal(); texAmp = 0; // extra precaution in case plane happens to // become (0,1,0,0) we don't want to suddenly add // texture } else { // adjust things so plane update for parent node preserves // projection of phantom position in our coordinate system // if (dynamicParent){ if (newCoordinates){ t_elapsed = 0; newCoordinates = FALSE; } else { t_elapsed += deltaT; } vrpn_HapticPosition posInPreviousCoordinates; PHANToM->getPosition_WC(posInPreviousCoordinates); posInPreviousCoordinates = dynamicParent->fromWorldLast(posInPreviousCoordinates); posInPreviousCoordinates = fromParent(posInPreviousCoordinates); if (posInPreviousCoordinates != phantomPos){ // if (t_elapsed > 0.0){ // printf("big t_elapsed: %f\n", t_elapsed); // } vrpn_HapticPosition currProj = plane.projectPoint(phantomPos); vrpn_HapticPosition prevProj = plane.projectPoint(posInPreviousCoordinates); // this is the projection of the // current position onto the // previous plane (as defined by // the parent transformation) if (!dynamicParent->dynamicMoveThisServoLoop()) printf("error: dynamicParent move not detected\n"); textureOrigin[0] = textureOrigin[0] - prevProj[0] + currProj[0]; textureOrigin[1] = 0; textureOrigin[2] = textureOrigin[2] - prevProj[2] + currProj[2]; } } // move textureOrigin from its current location towards current // position in steps of length texWL until it is within texWL of // the current position updateTextureOrigin(phantomPos[0], phantomPos[2]); // translate to texture coordinates - note: this is a translation in the plane of // the phantom position vrpn_HapticPosition texturePos = phantomPos - textureOrigin; SCP = computeSCPfromGradient(texturePos); vrpn_HapticPlane texPlane; texPlane = computeTangentPlaneAt(texturePos); depth = -(texturePos.x()*texPlane.a() + texturePos.y()*texPlane.b() + texturePos.z()*texPlane.c() + texPlane.d()); // go back to untranslated (non-texture) plane coordinates SCP = SCP + textureOrigin; SCPnormal = texPlane.normal(); // if (|texturePos| is close to 0.25*texWL or 0.75*texWL then we // are near a zero crossing so its relatively safe to // update texture shape double old_wl = texWL; double radius = sqrt(texturePos[0]*texturePos[0] + texturePos[2]*texturePos[2]);// this is between 0 and texWL if (fabs(radius - 0.25*texWL) < deltaDist || fabs(radius - 0.75*texWL) < deltaDist){ if (texWN_needs_update) updateTextureWavelength(); if (texAmp_needs_update) updateTextureAmplitude(); if (texSize_needs_update) updateTextureSize(); if (texAspect_needs_update) updateTextureAspectRatio(); if (old_wl != texWL && old_wl != 0){ // adjust to maintain phase continuity textureOrigin[0] = -texWL*texturePos[0]/old_wl + phantomPos[0]; textureOrigin[2] = -texWL*texturePos[2]/old_wl + phantomPos[2]; } } } if (depth <= 0) { // positive depth is below surface inContact = FALSE; lastDepth = 0; safety_ineffect = FALSE; (void) updateStateForPHANToM(PHANToM,inContact); return inContact; } // if we suddenly get a new plane that causes the depth in the // plane to be large then we don't want to let the phantom force // get large else if (safety_ineffect) { inContact = FALSE; lastDepth = 0; (void) updateStateForPHANToM(PHANToM,inContact); return inContact; } else if ((depth - lastDepth)*getSurfaceKspring() > MAX_FORCE){ inContact = FALSE; lastDepth = 0; fprintf(stderr, "Warning: exceeded max force change\n"); fprintf(stderr, " move out of surface to re-enable forces\n"); safety_ineffect = TRUE; (void) updateStateForPHANToM(PHANToM,inContact); return inContact; } else { // In contact inContact = TRUE; (void)updateStateForPHANToM(PHANToM, inContact); addCollision(PHANToM,SCP,SCPnormal); } // this value is important for safety checks lastDepth = depth; return inContact; } #endif void TexturePlane::incrementFade(double dT){ double curr_spr = getSurfaceKspring(); double next_spr = curr_spr - dT*dSpring_dt; if (next_spr > 0) setSurfaceKspring(next_spr); else{ inEffect = FALSE; fadeActive = FALSE; setSurfaceKspring(fadeOldKspring); } }
bool MeshShape::checkCollisionPeer(MeshShape *target) { float normal[3]; float contactpoint[3]; bool collided = false; int i; Mesh *sourcemesh, *targetmesh; sourcemesh = this->mesh; targetmesh = target->mesh; for (i = 0; i < sourcemesh->vertexcount; i++) { Vertex *vertex = &sourcemesh->vertices[i]; float vertexposition[3]; object->transformPoint(vertexposition, vertex->position); target->object->unTransformPoint(vertexposition, vertexposition); if (checkPointMeshCollision(vertexposition, targetmesh, normal, contactpoint)) { target->object->transformVector(normal, normal); target->object->transformPoint(contactpoint, contactpoint); if (vectorIsZero(contactpoint)) { vectorSet(contactpoint, 0, 0, 0); } addCollision(object, target->object, normal, contactpoint); collided = true; } } sourcemesh = target->mesh; targetmesh = this->mesh; for (i = 0; i < sourcemesh->vertexcount; i++) { Vertex *vertex = &sourcemesh->vertices[i]; float vertexposition[3]; target->object->transformPoint(vertexposition, vertex->position); object->unTransformPoint(vertexposition, vertexposition); if (checkPointMeshCollision(vertexposition, targetmesh, normal, contactpoint)) { object->transformVector(normal, normal); object->transformPoint(contactpoint, contactpoint); addCollision(target->object, object, normal, contactpoint); collided = true; } } sourcemesh = this->mesh; targetmesh = target->mesh; for (i = 0; i < sourcemesh->edgecount; i++) { Edge *edge = &sourcemesh->edges[i]; float v1[3], v2[3]; object->transformPoint(v1, edge->v1->position); target->object->unTransformPoint(v1, v1); object->transformPoint(v2, edge->v2->position); target->object->unTransformPoint(v2, v2); if (checkEdgeMeshCollision(v1, v2, targetmesh, normal, contactpoint)) { target->object->transformVector(normal, normal); target->object->transformPoint(contactpoint, contactpoint); addCollision(object, target->object, normal, contactpoint); collided = true; } } return collided; }