vrpn_HapticVector InstantBuzzEffect::calcEffectForce(void) {
#else
gstVector InstantBuzzEffect::calcEffectForce(void *phantom) {
#endif
// No force if inactive
if (!active) {
return vrpn_HapticVector(0,0,0);
}
LARGE_INTEGER counter;
if (currentPerformanceFrequency.HighPart == 0 && currentPerformanceFrequency.LowPart == 0) {
return vrpn_HapticVector(0,0,0);
}
#ifdef _WIN32
if (QueryPerformanceCounter(&counter) != TRUE){
fprintf(stderr, "unable to get perfo counter\n");
return vrpn_HapticVector(0,0,0);
}
#endif
double elapsedSec = (counter.QuadPart - debut.QuadPart) / (double) currentPerformanceFrequency.QuadPart;
if (elapsedSec < getDuration()) {
double currentPart = (counter.QuadPart*getFrequency()/currentPerformanceFrequency.QuadPart);
currentPart = (currentPart-(long)currentPart)*2*PI;
double currentForce = sin(currentPart);
double force = currentForce*getAmplitude();
return vrpn_HapticVector( x*force, y*force, z*force );
}
return vrpn_HapticVector(0,0,0);
}
// for texture:
vrpn_HapticVector TexturePlane::computeNormal(double x, double z) const
{
double k = 2*VRPN_PI*texWN;
double r_sq = x*x + z*z;
double r = sqrt(r_sq);
vrpn_HapticVector normal;
if (r != 0){
normal = vrpn_HapticVector(texAmp*x*k*sin(k*r)/r, 1.0, texAmp*z*k*sin(k*r)/r);
}
else{
normal = vrpn_HapticVector(0, 1.0, 0);
}
return normal;
}
vrpn_HapticVector InstantBuzzEffect::calcEffectForce(void) {
#else
gstVector InstantBuzzEffect::calcEffectForce(void *phantom) {
#endif
// No force if inactive
if (!active) {
return vrpn_HapticVector(0,0,0);
}
// XXX Needs to be implemented on non-Windows platforms. Use
// the gettimeofday() function to calculate timing on those platforms.
// We might want to switch to vrpn_gettimeofday() on all platforms, since
// that is now implemented on Windows.
#ifdef _WIN32
LARGE_INTEGER counter;
if (currentPerformanceFrequency.HighPart == 0 && currentPerformanceFrequency.LowPart == 0) {
return vrpn_HapticVector(0,0,0);
}
if (QueryPerformanceCounter(&counter) != TRUE){
fprintf(stderr, "unable to get perfo counter\n");
return vrpn_HapticVector(0,0,0);
}
double elapsedSec = (counter.QuadPart - debut.QuadPart) / (double) currentPerformanceFrequency.QuadPart;
if (elapsedSec < getDuration()) {
double currentPart = (counter.QuadPart*getFrequency()/currentPerformanceFrequency.QuadPart);
currentPart = (currentPart-(long)currentPart)*2*PI;
double currentForce = sin(currentPart);
double force = currentForce*getAmplitude();
return vrpn_HapticVector( x*force, y*force, z*force );
}
#endif
return vrpn_HapticVector(0,0,0);
}
vrpn_HapticVector ForceFieldEffect::calcEffectForce(void *phantom_info) {
vrpn_HapticPosition phantomPos;
vrpn_HapticVector effectForce = vrpn_HapticVector(0,0,0);
double forceMag;
int i,j;
if (active) {
// Find out the vector from the current position to the origin
// of the defined force field.
#ifdef VRPN_USE_HDAPI
HDAPI_state *state = (HDAPI_state *)phantom_info;
double vec[3];
vec[0] = state->pose[3][0]; vec[1] = state->pose[3][1]; vec[2] = state->pose[3][2];
phantomPos = vrpn_HapticPosition(vec);
#else
gstPHANToM *phantom = (gstPHANToM *)phantom_info;
phantom->getPosition_WC(phantomPos);
#endif
vrpn_HapticVector dR = (phantomPos - origin);
// If the Phantom has been moved too far from the origin,
// drop the force to zero.
#ifdef VRPN_USE_HDAPI
if (dR.magnitude() > radius) {
#else
if (dR.norm() > radius) {
#endif
return vrpn_HapticVector(0,0,0);
}
// Compute the force, which is the constant force plus
// a force that varies as the Phantom position deviates
// from the origin of the force field. The Jacobian
// determines how the force changes in different directions
// away from the origin (generalizing spring forces of different
// magnitudes that constrain the Phantom to the point of the
// origin, to a line containing the origin, or to a plane
// containing the origin). Tom Hudson worked out the math
// on this.
effectForce = force;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
effectForce[i] += dR[j]*jacobian[i][j];
}
}
// Clamp to FF_MAX_FORCE if it is exceeded, leaving the
// direction of the force unchanged.
#ifdef VRPN_USE_HDAPI
forceMag = effectForce.magnitude();
#else
forceMag = effectForce.norm();
#endif
if (forceMag > FF_MAX_FORCE) {
for (i = 0; i < 3; i++) {
effectForce[i] *= FF_MAX_FORCE/(forceMag);
}
}
}
// Return the force, which will be zero by default if the
// force field is not active.
return effectForce;
}
TexturePlane::TexturePlane(double a,double b, double c, double d)
{
init();
vrpn_HapticVector vec = vrpn_HapticVector(a,b,c);
plane = vrpn_HapticPlane(vec,d);
}
void DynamicPlane::planeEquationToTransform(vrpn_HapticPlane &prev_plane,
vrpn_HapticPlane &next_plane, vrpn_HapticMatrix &xform_to_update)
{
// plane is [a,b,c,d] where ax + by +cz + d = 0
vrpn_HapticVector next_normal = next_plane.normal();
vrpn_HapticVector prev_normal = prev_plane.normal();
#ifdef VRPN_USE_HDAPI
double m = next_normal.magnitude();
#else
double m = next_normal.distToOrigin();
#endif
double distAlongNormal = -next_plane.d();
vrpn_HapticVector axis = vrpn_HapticVector(0,0,0);
double theta = 0;
vrpn_HapticVector trans;
trans = distAlongNormal*next_normal; // translation vector for xform
// compute the rotation axis and angle
#ifdef VRPN_USE_HDAPI
vrpn_HapticVector crossprod = prev_normal.crossProduct(next_normal);
double sintheta = crossprod.magnitude();
double costheta = prev_normal.dotProduct(next_normal);
#else
vrpn_HapticVector crossprod = prev_normal.cross(next_normal);
double sintheta = crossprod.distToOrigin();
double costheta = prev_normal.dot(next_normal);
#endif
if (sintheta == 0) { // exceptional case
// pick an arbitrary vector perp to normal because we are either
// rotating by 0 or 180 degrees
if (next_normal[0] != 0) {
axis = vrpn_HapticVector(next_normal[1], -next_normal[0], 0);
#ifdef VRPN_USE_HDAPI
axis /= (axis.magnitude());
#else
axis /= (axis.distToOrigin());
#endif
} else {
axis = vrpn_HapticVector(1,0,0);
}
} else { // normal case
axis = crossprod/sintheta;
}
theta = asin(sintheta);
if (costheta < 0) theta = VRPN_PI - theta;
// set the incremental rotation
#ifdef VRPN_USE_HDAPI
vrpn_HapticMatrix rot_incrxform;
rot_incrxform.createRotation(axis, theta);
#else
vrpn_HapticMatrix rot_incrxform;
rot_incrxform.setRotate(axis,theta);
#endif
// Set the rotation portion of the matrix based on the accumulation
// of its previous value and the new rotation.
double prod[3][3];
int i,j;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
prod[i][j] = xform_to_update[i][0]*rot_incrxform[0][j] +
xform_to_update[i][1]*rot_incrxform[1][j] +
xform_to_update[i][2]*rot_incrxform[2][j];
}
}
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
xform_to_update[i][j] = prod[i][j];
}
}
// Set the translation for the new matrix. XXX For HDAPI, I'm not sure
// we are putting the translation into the proper portion of the
// matrix.
#ifdef VRPN_USE_HDAPI
for (i = 0; i < 3; i++) {
xform_to_update[3][i] = trans[i];
}
#else
xform_to_update.setTranslate(trans);
#endif
return;
}
