/******************************************************************************
Calculates updated object transform for drag object based on changes to
proxy transform.
******************************************************************************/
void updateDragObjectTransform(Mesh* pObj)
{
// Calculated delta between current proxy pos and proxy poas at start
// of drag.
hduVector3Dd proxyPos;
hlGetDoublev(HL_PROXY_POSITION, proxyPos);
hduVector3Dd dragDeltaTransl = proxyPos - pObj->startDragProxyPos;
// Same for rotation.
hduMatrix deltaRotMat;
// if (rotate)
{
hduQuaternion proxyRotq;
hlGetDoublev(HL_PROXY_ROTATION, proxyRotq);
hduQuaternion dragDeltaRot =
pObj->startDragProxyRot.inverse() * proxyRotq;
dragDeltaRot.normalize();
dragDeltaRot.toRotationMatrix(deltaRotMat);
// Want to rotate about the proxy position, not the origin
// so need to translate to/from proxy pos.
hduMatrix toProxy = hduMatrix::createTranslation(-pObj->startDragProxyPos);
hduMatrix fromProxy = hduMatrix::createTranslation(pObj->startDragProxyPos);
deltaRotMat = toProxy * deltaRotMat * fromProxy;
}
// Compose rotation and translation deltas.
hduMatrix deltaMat =
deltaRotMat * hduMatrix::createTranslation(dragDeltaTransl);
pObj->transform = pObj->startDragObjTransform * deltaMat;
}
/*******************************************************************************
The main routine for rendering scene haptics.
*******************************************************************************/
void drawSceneHaptics()
{
hlGetDoublev(HL_PROXY_TOUCH_NORMAL, proxyNormal);
hlGetDoublev(HL_PROXY_POSITION, proxyPosition);
// Start haptic frame. (Must do this before rendering any haptic shapes.)
hlBeginFrame();
hlPushMatrix();
hlTouchModel(HL_CONTACT);
// Set material properties for the shapes to be drawn.
hlMaterialf(HL_FRONT_AND_BACK, HL_STIFFNESS, 0.9f);
hlMaterialf(HL_FRONT_AND_BACK, HL_DAMPING, 0.0f);
hlMaterialf(HL_FRONT_AND_BACK, HL_STATIC_FRICTION, 0.1);
hlMaterialf(HL_FRONT_AND_BACK, HL_DYNAMIC_FRICTION,0.1 );
hlHinti(HL_SHAPE_FEEDBACK_BUFFER_VERTICES, objmodel->numvertices);
hlBeginShape(HL_SHAPE_FEEDBACK_BUFFER, gShapeId);
// Render haptic shape
glPushMatrix();
glCallList(bumpList);
glPopMatrix();
// End the shape.
hlEndShape();
hlPopMatrix();
hlPushMatrix();
hlTouchModel(HL_CONSTRAINT);
hlMaterialf(HL_FRONT_AND_BACK, HL_STIFFNESS, 0.4f);
hlMaterialf(HL_FRONT_AND_BACK, HL_DAMPING, 0.3f);
hlMaterialf(HL_FRONT_AND_BACK, HL_STATIC_FRICTION, 0.1);
hlMaterialf(HL_FRONT_AND_BACK, HL_DYNAMIC_FRICTION,0.1 );
if (touchedHole && force[2] > -0.1 )
{
hlTouchModelf(HL_SNAP_DISTANCE, 300.0);
}
else
{
hlTouchModelf(HL_SNAP_DISTANCE, 3.0);
}
hlBeginShape(HL_SHAPE_FEEDBACK_BUFFER, gPointId);
glPushMatrix();
glPointSize(5.0);
glTranslatef(0.0, 0.0, 1.0);
glBegin(GL_POINTS);
glVertex3f(0.05,-0.175,-0.975);
glEnd();
glPopMatrix();
hlEndShape();
hlPopMatrix();
// End the haptic frame.
hlEndFrame();
}
hduVector3Dd CHapticViewerView::getProxyPositionInModelCoord(
Mesh* pObj
)
{
hduMatrix mat = pObj->transform.getInverse();
hduVector3Dd proxyPos;
hlGetDoublev(HL_PROXY_POSITION, proxyPos);
hduVector3Dd posMC;
mat.multVecMatrix(proxyPos, posMC);
return posMC;
}
void CHapticViewerView::drawCursorGL()
{
static const double kCursorRadius = 0.5;
static const double kCursorHeight = 1.5;
static const int kCursorTess = 15;
HLdouble proxyxform[16];
GLUquadricObj *qobj = 0;
glPushAttrib(GL_CURRENT_BIT | GL_ENABLE_BIT | GL_LIGHTING_BIT);
glPushMatrix();
if (!m_cursorDisplayList)
{
m_cursorDisplayList = glGenLists(1);
glNewList(m_cursorDisplayList, GL_COMPILE);
qobj = gluNewQuadric();
gluSphere(qobj, kCursorRadius, kCursorTess, kCursorTess);
gluDeleteQuadric(qobj);
glEndList();
}
// Get the proxy transform in world coordinates.
hlGetDoublev(HL_PROXY_TRANSFORM, proxyxform);
glMultMatrixd(proxyxform);
// Apply the local cursor scale factor.
glScaled(m_cursorScale, m_cursorScale, m_cursorScale);
glEnable(GL_NORMALIZE);
glEnable(GL_COLOR_MATERIAL);
glColor3f(0.0f, 0.5f, 1.0f);
glCallList(m_cursorDisplayList);
glPopMatrix();
glPopAttrib();
}
/*
Function: RedrawCursor
Usage: RedrawCursor();
---------------------------------------------------------------------------
Displays a cursor using the current haptic device proxy transform and the
mapping between the workspace and world coordinates
*/
void RedrawCursor(void)
{
static const double kCursorRadius = 0.5;
static const int kCursorTess = 15;
HLdouble proxytransform[16];
GLUquadricObj *qobj = 0;
glPushAttrib(GL_CURRENT_BIT | GL_ENABLE_BIT | GL_LIGHTING_BIT);
glPushMatrix();
if (!mCursorDisplayList)
{
mCursorDisplayList = glGenLists(1);
glNewList(mCursorDisplayList, GL_COMPILE);
qobj = gluNewQuadric();
gluSphere(qobj, kCursorRadius, kCursorTess, kCursorTess);
gluDeleteQuadric(qobj);
glEndList();
}
/* Apply the local position/rotation transform of the haptic device proxy */
hlGetDoublev(HL_PROXY_TRANSFORM, proxytransform);
glMultMatrixd(proxytransform);
/* Apply the local cursor scale factor. */
glScaled(mCursorScale, mCursorScale, mCursorScale);
glEnable(GL_NORMALIZE);
glEnable(GL_COLOR_MATERIAL);
glColor3f(0.0, 0.5, 1.0);
glCallList(mCursorDisplayList);
glPopMatrix();
glPopAttrib();
}
void CProtoHapticDoc::SimulationStep()
{
int elapsed= clock() - m_lastSimStep;
int steps= (int)((((float)elapsed)*m_simSpeed)*0.1f);
if(steps<1) {
m_lastSimStep= clock();
return;
}
// collision detection
for(int i= 0; i<m_shapeCount; i++) {
if(m_shapes[i]->isCollisionDynamic())
dGeomSetBody (m_geoms[i], bodies[i]);
else
dGeomSetBody (m_geoms[i], 0);
}
dSpaceCollide (m_spaceID,this,&nearCallbackStatic);
dWorldQuickStep (m_worldID, steps);
dJointGroupEmpty (m_jointGroup);
for( int i= 0; i<m_shapeCount; i++) {
//air resistance
if(m_shapes[i]->isCollisionDynamic()||m_shapes[i]->isProxyDynamic()) {
const dReal *vel;
const dReal *angvel;
vel= dBodyGetLinearVel (bodies[i]);
dBodyAddForce (bodies[i], -vel[0]*m_airResistance, -vel[1]*m_airResistance, -vel[2]*m_airResistance);
angvel= dBodyGetAngularVel (bodies[i]);
dBodyAddTorque (bodies[i], -angvel[0]*m_airResistance, -angvel[1]*m_airResistance, -angvel[2]*m_airResistance);
}
HHLRC rc= hlGetCurrentContext();
// proxy0
hlMakeCurrent(m_context);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
if(((CProtoHapticApp*)AfxGetApp())->isTwoDevices()) {
// proxy1
hlMakeCurrent(m_context_1);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching1()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
}
hlMakeCurrent(rc);
// gravity
if(m_shapes[i]->isCollisionDynamic())
dBodyAddForce(bodies[i], 0.0, -m_shapes[i]->getMass()*(m_gravity/10.0f), 0.0);
const dReal *pos;
const dReal *rot;
pos= dBodyGetPosition (bodies[i]);
rot= dBodyGetRotation (bodies[i]);
float rotation[16];
rotation[0]= rot[0]; rotation[4]= rot[1]; rotation[8]= rot[2]; rotation[12]= rot[3];
rotation[1]= rot[4]; rotation[5]= rot[5]; rotation[9]= rot[6]; rotation[13]= rot[7];
rotation[2]= rot[8]; rotation[6]= rot[9]; rotation[10]= rot[10]; rotation[14]= rot[11];
rotation[3]= 0.0; rotation[7]= 0.0; rotation[11]= 0.0; rotation[15]= 1.0;
m_shapes[i]->setRotation(rotation);
m_shapes[i]->setLocation(pos[0], pos[1], pos[2]);
}
m_lastSimStep= clock();
}
VOID CALLBACK IntroTrial::TimerProc(HWND hwnd,
UINT uMsg,
UINT_PTR idEvent,
DWORD dwTime)
{
//KillTimer(hwnd, idEvent);
if (IntroTrial::CurrentTrial)
{
#ifndef NO_HAPTICS
HLdouble proxypos[3];
// Get proxy position in world coordinates.
hlGetDoublev(HL_PROXY_POSITION, proxypos);
#endif
int stage = IntroTrial::CurrentTrial->GetStage();
if (stage == 0)
{
// Check if within sensible place
// If so, set up gravity and move to stage 1
#ifndef NO_HAPTICS
if (proxypos[2] < -0.02)
#endif
{
IntroTrial::CurrentTrial->SetStage(1);
AppStateVars.FirstFrame = true;
}
}
else if (stage == 1)
{
#ifndef NO_HAPTICS
double dxsq = ((double) proxypos[0]) * ((double) proxypos[0]);
double dysq = ((double) proxypos[1]) * ((double) proxypos[1]);
double dz = ((double) proxypos[2]) - IntroTrial::ZoneOffset;
double dzsq = dz * dz;
double rad = sqrt(dxsq + dysq + dzsq);
bool withinzone = (rad < IntroTrial::ZoneRad);
// Check if within zone for long enough
if (IntroTrial::CurrentTrial->InZone())
{
if (withinzone)
{
if (IntroTrial::CurrentTrial->CalcZoneTime())
{
// Within zone for long enough!
AppStateVars.WritePosition();
// Notify end of trial.
IntroTrial::CurrentTrial->Notify();
}
}
else
{
IntroTrial::CurrentTrial->ZoneLeft();
}
}
else
{
if (withinzone)
{
IntroTrial::CurrentTrial->ZoneEntered();
}
}
}
#else
}
IntroTrial::CurrentTrial->Notify();
#endif
}
/*******************************************************************************
Draws a 3D cursor for the haptic device using the current local transform,
the workspace to world transform and the screen coordinate scale.
*******************************************************************************/
void drawCursor()
{
static const double kCursorRadius = 0.25;
static const double kCursorHeight = 1.5;
static const int kCursorTess = 15;
HLdouble proxyxform[16];
double proxyPos[3];
hlGetDoublev(HL_PROXY_POSITION, proxyPos);
GLUquadricObj *qobj = 0;
glPushAttrib(GL_CURRENT_BIT | GL_ENABLE_BIT | GL_LIGHTING_BIT);
glPushMatrix();
if (!gCursorDisplayList)
{
gCursorDisplayList = glGenLists(1);
glNewList(gCursorDisplayList, GL_COMPILE);
qobj = gluNewQuadric();
gluCylinder(qobj, 0.0, kCursorRadius, kCursorHeight,
kCursorTess, kCursorTess);
glTranslated(0.0, 0.0, kCursorHeight);
gluCylinder(qobj, kCursorRadius, 0.0, kCursorHeight / 5.0,
kCursorTess, kCursorTess);
gluDeleteQuadric(qobj);
glEndList();
}
// Get the proxy transform in world coordinates.
hlGetDoublev(HL_PROXY_TRANSFORM, proxyxform);
//If entered hole, then freeze the rotations.
if (touchedHole)
{
proxyxform[0] = 1.0;
proxyxform[1] = 0.0;
proxyxform[2] = 0.0;
proxyxform[3] = 0.0;
proxyxform[4] = 0.0;
proxyxform[5] = 1.0;
proxyxform[6] = 0.0;
proxyxform[7] = 0.0;
proxyxform[8] = 0.0;
proxyxform[9] = 0.0;
proxyxform[10] = 1.0;
proxyxform[11] = 0.0;
}
//Get the depth of Penetration from HLAPI.
hlGetDoublev(HL_DEPTH_OF_PENETRATION, &probeDop);
glMultMatrixd(proxyxform);
if (touchedHole)
cursorToToolTranslation = 0.35;
else
cursorToToolTranslation = 0.25;
glTranslatef(0.0,0.0,cursorToToolTranslation);
if (touchedHole && force[2]>=0.0)
glTranslatef(0.0,0.0,-1* probeDop);
glCallList(toolObjList);
glPopMatrix();
glPopAttrib();
}
