// Wraps objects that go outside the screen's bounds
void wrapPosition(object *o) {
// Convert object position to screen space
vector toCheck = fromWorld(&(o->pos));
// If a laser, deactivate the object
if (o->type == LAZOR &&
(toCheck.x > 1.0 || toCheck.x < -1.0 ||
toCheck.y > 1.0 || toCheck.y < -1.0)) {
o->state &= !LAZER_ACTIVE;
return;
}
// Adjust position
if (toCheck.x > 1.0) toCheck.x = -1.0;
else if (toCheck.x < -1.0) toCheck.x = 1.0;
if (toCheck.y > 1.0) toCheck.y = -1.0;
else if (toCheck.y < -1.0) toCheck.y = 1.0;
// Convert position back to world space
o->pos = toWorld(&toCheck);
}
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);
}
}
