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;
}
