void CQTOpenGLUserFunctions::DrawCircle(const CVector3& c_position,
const CQuaternion& c_orientation,
Real f_radius,
const CColor& c_color,
const bool b_fill,
GLuint un_vertices) {
/* Save attributes and current matrix */
glPushAttrib(GL_POLYGON_BIT);
/* Set color */
SetColor(c_color);
/* Disable face culling, to make the triangle visible from any angle */
glDisable(GL_CULL_FACE);
/* Set polygon attributes */
glEnable(GL_POLYGON_SMOOTH);
glPolygonMode(GL_FRONT_AND_BACK, b_fill ? GL_FILL : GL_LINE);
/* Set position/orientation */
Rototranslate(c_position, c_orientation);
/* Draw */
CVector2 cVertex(f_radius, 0.0f);
CRadians cAngle(CRadians::TWO_PI / un_vertices);
glBegin(GL_POLYGON);
glNormal3f(0.0f, 0.0f, 1.0f);
for(size_t i = 0; i < un_vertices; ++i) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), 0.0f);
cVertex.Rotate(cAngle);
}
glEnd();
/* Restore saved stuff */
glPopAttrib();
}
void CQTOpenGLEPuck::RenderLED() {
/* Side surface */
CVector2 cVertex(BODY_RADIUS, 0.0f);
CRadians cAngle(CRadians::TWO_PI / m_unVertices);
CVector2 cNormal(1.0f, 0.0f);
glBegin(GL_QUAD_STRIP);
for(GLuint i = 0; i <= m_unVertices / 8; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), 0.0f);
glVertex3f(cVertex.GetX(), cVertex.GetY(), LED_ELEVATION + LED_HEIGHT);
glVertex3f(cVertex.GetX(), cVertex.GetY(), LED_ELEVATION);
cVertex.Rotate(cAngle);
cNormal.Rotate(cAngle);
}
glEnd();
/* Top surface */
cVertex.Set(BODY_RADIUS, 0.0f);
CVector2 cVertex2(LED_UPPER_RING_INNER_RADIUS, 0.0f);
glBegin(GL_QUAD_STRIP);
glNormal3f(0.0f, 0.0f, 1.0f);
for(GLuint i = 0; i <= m_unVertices / 8; i++) {
glVertex3f(cVertex2.GetX(), cVertex2.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
cVertex.Rotate(cAngle);
cVertex2.Rotate(cAngle);
}
glEnd();
}
//===========================================================================
void cDirectionalLight::setDir(const cVector3d& a_direction)
{
// We arbitrarily point lights along the x axis of the stored
// rotation matrix... this allows matrix transformations
// to apply to lights.
// m_localRot.setCol0(a_direction);
cVector3d c0, c1, c2, t0, t1;
a_direction.copyto(c0);
// check vector
if (c0.lengthsq() < 0.0001) {
return;
}
// normalize direction vector
c0.normalize();
// compute 2 vector perpendicular to a_direction
t0.set(0.0, 0.0, 1.0);
t1.set(0.0, 1.0, 0.0);
double a0 = cAngle(c0, t0);
double a1 = cAngle(c0, t1);
if (sin(a0) > sin(a1))
{
c0.crossr(t0, c1);
c0.crossr(c1, c2);
}
else
{
c0.crossr(t1, c1);
c0.crossr(c1, c2);
}
c1.normalize();
c2.normalize();
// update rotation matrix
m_localRot.setCol(c0,c1,c2);
}
void CQTOpenGLCylinder::MakeBody() {
/* Since this shape can be stretched,
make sure the normal vectors are unit-long */
glEnable(GL_NORMALIZE);
/* Set the material */
glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SPECULAR);
glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, SHININESS);
glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, EMISSION);
/* Let's start the actual shape */
/* Side surface */
CVector2 cVertex(1.0f, 0.0f);
CRadians cAngle(CRadians::TWO_PI / m_unVertices);
glBegin(GL_QUAD_STRIP);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cVertex.GetX(), cVertex.GetY(), 0.0f);
glVertex3f(cVertex.GetX(), cVertex.GetY(), 1.0f);
glVertex3f(cVertex.GetX(), cVertex.GetY(), 0.0f);
cVertex.Rotate(cAngle);
}
glEnd();
/* Top disk */
cVertex.Set(1.0f, 0.0f);
glBegin(GL_POLYGON);
glNormal3f(0.0f, 0.0f, 1.0f);
for(GLuint i = 0; i <= m_unVertices; i++) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), 1.0f);
cVertex.Rotate(cAngle);
}
glEnd();
/* Bottom disk */
cVertex.Set(1.0f, 0.0f);
cAngle = -cAngle;
glBegin(GL_POLYGON);
glNormal3f(0.0f, 0.0f, -1.0f);
for(GLuint i = 0; i <= m_unVertices; i++) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), 0.0f);
cVertex.Rotate(cAngle);
}
glEnd();
/* The shape definition is finished */
/* We don't need it anymore */
glDisable(GL_NORMALIZE);
}
void CQTOpenGLUserFunctions::DrawCircle(Real f_radius,
const CVector3& c_center_offset,
const CColor& c_color,
const bool b_fill,
const CQuaternion& c_orientation,
GLuint un_vertices) {
glDisable(GL_LIGHTING);
glDisable(GL_CULL_FACE);
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
CVector3 cVertex(f_radius, 0.0f, 0.0f);
CRadians cAngle(CRadians::TWO_PI / un_vertices);
if(b_fill) {
glBegin(GL_POLYGON);
}
else {
glBegin(GL_LINE_LOOP);
}
CVector3 cNormalDirection(0.0f, 0.0f, 1.0f);
cNormalDirection.Rotate(c_orientation);
glNormal3f(cNormalDirection.GetX(),
cNormalDirection.GetY(),
cNormalDirection.GetZ());
/* Compute the quaternion defining the rotation of the vertices used to draw the circle. */
CQuaternion cVertexRotation;
CVector3 cVertexRotationAxis(0.0f, 0.0f, 1.0f);
cVertexRotationAxis.Rotate(c_orientation);
cVertexRotation.FromAngleAxis(cAngle, cVertexRotationAxis);
cVertex.Rotate(c_orientation);
for(GLuint i = 0; i <= un_vertices; i++) {
glVertex3f(cVertex.GetX() + c_center_offset.GetX(),
cVertex.GetY() + c_center_offset.GetY(),
cVertex.GetZ() + c_center_offset.GetZ());
cVertex.Rotate(cVertexRotation);
}
glEnd();
glEnable(GL_CULL_FACE);
glEnable(GL_LIGHTING);
}
void CQTOpenGLUserFunctions::DrawCylinder(const CVector3& c_position,
const CQuaternion& c_orientation,
Real f_radius,
Real f_height,
const CColor& c_color,
GLuint un_vertices) {
/* Save current matrix */
glPushMatrix();
/* Set color */
SetColor(c_color);
/* Set position/orientation */
Rototranslate(c_position, c_orientation);
/* Draw side surface */
Real fHalfHeight = f_height * 0.5f;
CVector2 cVertex(1.0f, 0.0f);
CRadians cAngle(CRadians::TWO_PI / un_vertices);
glBegin(GL_QUAD_STRIP);
for(GLuint i = 0; i <= un_vertices; i++) {
glNormal3f(cVertex.GetX(), cVertex.GetY(), 0.0f);
glVertex3f(cVertex.GetX() * f_radius, cVertex.GetY() * f_radius, fHalfHeight);
glVertex3f(cVertex.GetX() * f_radius, cVertex.GetY() * f_radius, -fHalfHeight);
cVertex.Rotate(cAngle);
}
glEnd();
/* Draw top disk */
cVertex.Set(f_radius, 0.0f);
glBegin(GL_POLYGON);
glNormal3f(0.0f, 0.0f, 1.0f);
for(GLuint i = 0; i <= un_vertices; i++) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), fHalfHeight);
cVertex.Rotate(cAngle);
}
glEnd();
/* Draw bottom disk */
cVertex.Set(f_radius, 0.0f);
cAngle = -cAngle;
glBegin(GL_POLYGON);
glNormal3f(0.0f, 0.0f, -1.0f);
for(GLuint i = 0; i <= un_vertices; i++) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), -fHalfHeight);
cVertex.Rotate(cAngle);
}
glEnd();
/* Restore saved matrix */
glPopMatrix();
}
void CQTOpenGLEPuck::RenderBody() {
/* Set material */
SetGreenPlasticMaterial();
CVector2 cVertex(BODY_RADIUS, 0.0f);
CRadians cAngle(-CRadians::TWO_PI / m_unVertices);
/* Bottom part */
glBegin(GL_POLYGON);
glNormal3f(0.0f, 0.0f, -1.0f);
for(GLuint i = 0; i <= m_unVertices; i++) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION);
cVertex.Rotate(cAngle);
}
glEnd();
/* Side surface */
cAngle = -cAngle;
CVector2 cNormal(1.0f, 0.0f);
cVertex.Set(BODY_RADIUS, 0.0f);
glBegin(GL_QUAD_STRIP);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), 0.0f);
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT);
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION);
cVertex.Rotate(cAngle);
cNormal.Rotate(cAngle);
}
glEnd();
/* Top part */
glBegin(GL_POLYGON);
cVertex.Set(LED_UPPER_RING_INNER_RADIUS, 0.0f);
glNormal3f(0.0f, 0.0f, 1.0f);
for(GLuint i = 0; i <= m_unVertices; i++) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
cVertex.Rotate(cAngle);
}
glEnd();
/* Triangle to set the direction */
SetLEDMaterial(1.0f, 1.0f, 0.0f);
glBegin(GL_TRIANGLES);
glVertex3f( BODY_RADIUS * 0.7, 0.0f, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
glVertex3f(-BODY_RADIUS * 0.7, BODY_RADIUS * 0.3, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
glVertex3f(-BODY_RADIUS * 0.7, -BODY_RADIUS * 0.3, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
glEnd();
}
void CQTOpenGLEPuck::RenderWheel() {
/* Set material */
SetRedPlasticMaterial();
/* Right side */
CVector2 cVertex(WHEEL_RADIUS, 0.0f);
CRadians cAngle(CRadians::TWO_PI / m_unVertices);
CVector3 cNormal(-1.0f, -1.0f, 0.0f);
cNormal.Normalize();
glBegin(GL_POLYGON);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
glVertex3f(cVertex.GetX(), -HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
cVertex.Rotate(cAngle);
cNormal.RotateY(cAngle);
}
glEnd();
/* Left side */
cVertex.Set(WHEEL_RADIUS, 0.0f);
cNormal.Set(-1.0f, 1.0f, 0.0f);
cNormal.Normalize();
cAngle = -cAngle;
glBegin(GL_POLYGON);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
glVertex3f(cVertex.GetX(), HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
cVertex.Rotate(cAngle);
cNormal.RotateY(cAngle);
}
glEnd();
/* Tire */
cNormal.Set(1.0f, 0.0f, 0.0f);
cVertex.Set(WHEEL_RADIUS, 0.0f);
cAngle = -cAngle;
glBegin(GL_QUAD_STRIP);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
glVertex3f(cVertex.GetX(), -HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
glVertex3f(cVertex.GetX(), HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
cVertex.Rotate(cAngle);
cNormal.RotateY(cAngle);
}
glEnd();
}
//===========================================================================
void cODEGenericBody::createStaticPlane(const cVector3d& a_position,
const cVector3d& a_normal)
{
// object is static by default
m_static = true;
// temp variables
cVector3d normal = a_normal;
cVector3d offset(0,0,0);
// check normal
if (normal.length() == 0) { return; }
// compute parameters
normal.normalize();
double a = normal.x;
double b = normal.y;
double c = normal.z;
offset = cProject(a_position, normal);
double d = offset.length();
if (d > 0)
{
if (cAngle(offset, normal) > cDegToRad(90))
{
d = -d;
}
}
// build fixed plane
m_ode_geom = dCreatePlane(m_ODEWorld->m_ode_space, a, b, c, d);
// store dynamic model type
m_typeDynamicCollisionModel = ODE_MODEL_PLANE;
// store dynamic model parameters
m_paramDynColModel0 = normal.x;
m_paramDynColModel1 = normal.y;
m_paramDynColModel2 = normal.z;
m_posOffsetDynColModel = a_position;
m_rotOffsetDynColModel.identity();
}
void updateHaptics(void)
{
// main haptic simulation loop
while(simulationRunning)
{
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
// if the haptic device does track orientations, we automatically
// oriente the drill to remain perpendicular to the tooth
cVector3d pos = tool->m_proxyPointForceModel->getProxyGlobalPosition();
cMatrix3d rot = tool->m_deviceGlobalRot;
if (info.m_sensedRotation == false)
{
cVector3d pos = tool->m_proxyPointForceModel->getProxyGlobalPosition();
rot.identity();
cVector3d vx, vy, vz;
cVector3d zUp (0,0,1);
cVector3d yUp (0,1,0);
vx = pos - tooth->getPos();
if (vx.length() > 0.001)
{
vx.normalize();
if (cAngle(vx,zUp) > 0.001)
{
vy = cCross(zUp, vx);
vy.normalize();
vz = cCross(vx, vy);
vz.normalize();
}
else
{
vy = cCross(yUp, vx);
vy.normalize();
vz = cCross(vx, vy);
vz.normalize();
}
rot.setCol(vx, vy, vz);
drill->setRot(rot);
}
}
int button = tool->getUserSwitch(0);
if (button == 0)
{
lastPosDevice = pos;
lastRotDevice = rot;
lastPosObject = tooth->getPos();
lastRotObject = tooth->getRot();
lastDeviceObjectPos = cTrans(lastRotDevice) * ((lastPosObject - lastPosDevice) + 0.01*cNormalize(lastPosObject - lastPosDevice));
lastDeviceObjectRot = cMul(cTrans(lastRotDevice), lastRotObject);
tooth->setHapticEnabled(true, true);
tool->setShowEnabled(true, true);
drill->setShowEnabled(true, true);
}
else
{
tool->setShowEnabled(false, true);
drill->setShowEnabled(false, true);
cMatrix3d rotDevice01 = cMul(cTrans(lastRotDevice), rot);
cMatrix3d newRot = cMul(rot, lastDeviceObjectRot);
cVector3d newPos = cAdd(pos, cMul(rot, lastDeviceObjectPos));
tooth->setPos(newPos);
tooth->setRot(newRot);
world->computeGlobalPositions(true);
tooth->setHapticEnabled(false, true);
}
//-----------------Jake--------------//
char serialData[50];
double pos1 = 0;
double pos2 = 0;
if (sp->ReadData(serialData, strlen(serialData)) > 0)
{
if (sp->parse_num(serialData, pos1))
{
Sleep(50);
if (pos1 > 0.3)
{
pos1 = 0.126*pow(pos1,-1.07);
pos1 = (pos1+pos1_1+pos1_2)/3;
//pos1 = (pos1 + pos1_1 + pos1_2 + pos1_3 + pos1_4)/5;
pos1_5 = pos1_4;
pos1_4 = pos1_3;
pos1_3 = pos1_2;
pos1_2 = pos1_1;
pos1_1 = pos1;
pos2 = 0.126*pow(pos2,-1.07);
pos2 = (pos2 + pos2_1 + pos2_2 + pos2_3 + pos2_4)/5;
pos2_5 = pos2_4;
pos2_4 = pos2_3;
pos2_3 = pos2_2;
pos2_2 = pos2_1;
pos2_1 = pos2;
}
else
{
pos1 = previous_pos1;
pos2 = previous_pos2;
}
printf("Received data %f and %f \n", pos1, pos2);
} else
{
printf("Conversion failed\n");
}
} else {
printf("Receive failed\n");
}
if (pos_counter > 2)
{
double dx1 = pos1 - previous_pos1;
double dx2 = pos2 - previous_pos2;
if (abs(dx1) < 0.1)
{
overall_pos1 -= dx1*10;
if (overall_pos1 < -0.1)
{
overall_pos1 = -0.1;
} else if(overall_pos1 > 0)
{
overall_pos1 = 0;
}
}
if (abs(dx2) < 0.1)
{
overall_pos2 -= dx2;
if (overall_pos2 < -0.1)
{
overall_pos2 = -0.1;
} else if(overall_pos2 > 0)
{
overall_pos2 = 0;
}
}
} else {
pos_counter++;
}
previous_pos1 = pos1;
previous_pos2 = pos2;
index_finger->setPos(pos.x - 0.15, pos.y, pos.z + overall_pos1);
index_finger->setRot(rot);
index_finger->computeInteractionForces();
index_finger->updatePose();
thumb->setPos(pos.x + 0.1, pos.y - 0.15 + overall_pos2, pos.z - 0.05);
thumb->setRot(rot);
thumb->computeInteractionForces();
thumb->updatePose();
// compute global reference frames for each object
world->computeGlobalPositions(true);
std::stringstream torque_str_tmp;
double torque_temp1 = sqrt(pow(index_finger->m_lastComputedGlobalForce.x,2) + pow(index_finger->m_lastComputedGlobalForce.y,2) + pow(index_finger->m_lastComputedGlobalForce.z,2));
double torque_temp2 = sqrt(pow(thumb->m_lastComputedGlobalForce.x,2) + pow(thumb->m_lastComputedGlobalForce.y,2) + pow(thumb->m_lastComputedGlobalForce.z,2));
/*torque_str_tmp << std::setprecision(2) << torque_temp;
const std::string& torque_to_send = torque_str_tmp.str();
char to_send[50];
strcpy(to_send, "T");
strcat(to_send, torque_to_send.c_str());
strcat(to_send, ";");
*/
char to_send[50];
if (torque_temp1 > 0 && torque_temp2 > 0)
{
strcpy(to_send, "T");
strcat(to_send, "1/1");
strcat(to_send, ";");
} else if (torque_temp1 == 0 && torque_temp2 > 0)
{
strcpy(to_send, "T");
strcat(to_send, "0/1");
strcat(to_send, ";");
} else if (torque_temp1 > 0 && torque_temp2 == 0)
{
strcpy(to_send, "T");
strcat(to_send, "1/0");
strcat(to_send, ";");
} else
{
strcpy(to_send, "T");
strcat(to_send, "0/0");
strcat(to_send, ";");
}
if (sp->WriteData(to_send, strlen(to_send)))
{
printf("Sent %s\n", to_send);
} else
{
printf("Force data %s could not be sent\n", to_send);
}
}
// exit haptics thread
simulationFinished = true;
}
//===========================================================================
void cProxyPointForceAlgo::updateForce()
{
// initialize variables
double stiffness = 0.0;
cVector3d normal;
normal.zero();
// if there are no contacts between proxy and environment, no force is applied
if (m_numContacts == 0)
{
m_lastGlobalForce.zero();
return;
}
//---------------------------------------------------------------------
// stiffness and surface normal estimation
//---------------------------------------------------------------------
else if (m_numContacts == 1)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() );
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
}
// if there are two contact points, the stiffness is the average of the
// stiffnesses of the two intersected triangles' meshes
else if (m_numContacts == 2)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint1->m_triangle->getParent()->m_material.getStiffness() ) / 2.0;
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
normal.add(m_contactPoint1->m_globalNormal);
normal.mul(1.0/2.0);
}
// if there are three contact points, the stiffness is the average of the
// stiffnesses of the three intersected triangles' meshes
else if (m_numContacts == 3)
{
// compute stiffness
stiffness = ( m_contactPoint0->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint1->m_triangle->getParent()->m_material.getStiffness() +
m_contactPoint2->m_triangle->getParent()->m_material.getStiffness() ) / 3.0;
// compute surface normal
normal.add(m_contactPoint0->m_globalNormal);
normal.add(m_contactPoint1->m_globalNormal);
normal.add(m_contactPoint2->m_globalNormal);
normal.mul(1.0/3.0);
}
//---------------------------------------------------------------------
// computing a force (Hooke's law)
//---------------------------------------------------------------------
// compute the force by modeling a spring between the proxy and the device
cVector3d force;
m_proxyGlobalPos.subr(m_deviceGlobalPos, force);
force.mul(stiffness);
m_lastGlobalForce = force;
// compute tangential and normal forces
if ((force.lengthsq() > 0) && (m_numContacts > 0))
{
m_normalForce = cProject(force, normal);
force.subr(m_normalForce, m_tangentialForce);
}
else
{
m_tangentialForce.zero();
m_normalForce = force;
}
//---------------------------------------------------------------------
// force shading (optional)
//---------------------------------------------------------------------
if ((m_useForceShading) && (m_numContacts == 1))
{
// get vertices and normals related to contact triangle
cVector3d vertex0 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex0()->getPos()));
cVector3d vertex1 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex1()->getPos()));
cVector3d vertex2 = cAdd(m_contactPoint0->m_object->getGlobalPos(), cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex2()->getPos()));
cVector3d normal0 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex0()->getNormal());
cVector3d normal1 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex1()->getNormal());
cVector3d normal2 = cMul(m_contactPoint0->m_object->getGlobalRot(), m_contactPoint0->m_triangle->getVertex2()->getNormal());
// compute angles between normals. If the angles are very different, then do not apply shading.
double angle01 = cAngle(normal0, normal1);
double angle02 = cAngle(normal0, normal2);
double angle12 = cAngle(normal1, normal2);
if ((angle01 < m_forceShadingAngleThreshold) || (angle02 < m_forceShadingAngleThreshold) || (angle12 < m_forceShadingAngleThreshold))
{
double a0 = 0;
double a1 = 0;
cProjectPointOnPlane(m_contactPoint0->m_globalPos, vertex0, vertex1, vertex2, a0, a1);
cVector3d normalShaded = cAdd(
cMul(0.5, cAdd(cMul(a0, normal1), cMul((1-a0), normal0))),
cMul(0.5, cAdd(cMul(a1, normal2), cMul((1-a1), normal0)))
);
normalShaded.normalize();
if (cAngle(normalShaded, normal) > 1.0)
{
normalShaded.negate();
}
if (cAngle(normal, normalShaded) < m_forceShadingAngleThreshold)
{
double forceMagnitude = m_normalForce.length();
force = cAdd( cMul(forceMagnitude, normalShaded), m_tangentialForce);
m_lastGlobalForce = force;
normal = normalShaded;
// update tangential and normal forces again
if ((force.lengthsq() > 0) && (m_numContacts > 0))
{
m_normalForce = cProject(force, normal);
force.subr(m_normalForce, m_tangentialForce);
}
else
{
m_tangentialForce.zero();
m_normalForce = force;
}
}
}
}
}
//==============================================================================
void cToolGripper::computeInteractionForces()
{
// convert the angle of the gripper into a position in device coordinates.
// this value is device dependent.
double gripperPositionFinger = 0.0;
double gripperPositionThumb = 0.0;
if (m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_OMEGA_7)
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
else if (m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_SIGMA_7)
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
else
{
gripperPositionFinger = 0.040 * cSinRad( m_deviceGripperAngle + cDegToRad( 1.0));
gripperPositionThumb = 0.040 * cSinRad(-m_deviceGripperAngle + cDegToRad(-1.0));
}
// compute new position of thumb and finger
cVector3d lineFingerThumb = getGlobalRot().getCol1();
cVector3d pFinger = m_gripperWorkspaceScale * m_workspaceScaleFactor * gripperPositionFinger * lineFingerThumb;
cVector3d pThumb = m_gripperWorkspaceScale * m_workspaceScaleFactor * gripperPositionThumb * lineFingerThumb;
cVector3d posFinger, posThumb;
if (m_hapticDevice->m_specifications.m_rightHand)
{
posFinger = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (1.0 * pFinger));
posThumb = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (1.0 * pThumb));
}
else
{
posFinger = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (-1.0 * pFinger));
posThumb = m_deviceGlobalPos + cMul(m_deviceGlobalRot, (-1.0 * pThumb));
}
// compute forces
cVector3d forceThumb = m_hapticPointThumb->computeInteractionForces(posThumb,
m_deviceGlobalRot,
m_deviceGlobalLinVel,
m_deviceGlobalAngVel);
cVector3d forceFinger = m_hapticPointFinger->computeInteractionForces(posFinger,
m_deviceGlobalRot,
m_deviceGlobalLinVel,
m_deviceGlobalAngVel);
// compute torques
double scl = 0.0;
double factor = m_gripperWorkspaceScale * m_workspaceScaleFactor;
if (factor > 0.0)
{
scl = 1.0 / factor;
}
cVector3d torque = scl * cAdd(cCross(cSub(posThumb, m_deviceGlobalPos), forceThumb), cCross(cSub(posFinger, m_deviceGlobalPos), forceFinger));
// compute gripper force
double gripperForce = 0.0;
if ((m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_OMEGA_7) ||
(m_hapticDevice->m_specifications.m_model == C_HAPTIC_DEVICE_SIGMA_7))
{
cVector3d dir = posFinger - posThumb;
if (dir.length() > 0.00001)
{
dir.normalize ();
cVector3d force = cProject (forceFinger, dir);
gripperForce = force.length();
if (force.length() > 0.001)
{
double angle = cAngle(dir, force);
if ((angle > C_PI/2.0) || (angle < -C_PI/2.0)) gripperForce = -gripperForce;
}
}
}
// gripper damping
double gripperAngularVelocity = 0.0;
m_hapticDevice->getGripperAngularVelocity(gripperAngularVelocity);
double gripperDamping = -0.1 * m_hapticDevice->m_specifications.m_maxGripperAngularDamping * gripperAngularVelocity;
// finalize forces, torques and gripper force
m_lastComputedGlobalForce = forceThumb + forceFinger;
m_lastComputedGlobalTorque = torque;
m_lastComputedGripperForce = gripperForce + gripperDamping;
}
void updateHaptics(void)
{
// main haptic simulation loop
while(simulationRunning)
{
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
// if the haptic device does track orientations, we automatically
// oriente the drill to remain perpendicular to the tooth
cVector3d pos = tool->m_proxyPointForceModel->getProxyGlobalPosition();
cMatrix3d rot = tool->m_deviceGlobalRot;
if (info.m_sensedRotation == false)
{
cVector3d pos = tool->m_proxyPointForceModel->getProxyGlobalPosition();
rot.identity();
cVector3d vx, vy, vz;
cVector3d zUp (0,0,1);
cVector3d yUp (0,1,0);
vx = pos - tooth->getPos();
if (vx.length() > 0.001)
{
vx.normalize();
if (cAngle(vx,zUp) > 0.001)
{
vy = cCross(zUp, vx);
vy.normalize();
vz = cCross(vx, vy);
vz.normalize();
}
else
{
vy = cCross(yUp, vx);
vy.normalize();
vz = cCross(vx, vy);
vz.normalize();
}
rot.setCol(vx, vy, vz);
drill->setRot(rot);
}
}
int button = tool->getUserSwitch(0);
if (button == 0)
{
lastPosDevice = pos;
lastRotDevice = rot;
lastPosObject = tooth->getPos();
lastRotObject = tooth->getRot();
lastDeviceObjectPos = cTrans(lastRotDevice) * ((lastPosObject - lastPosDevice) + 0.01*cNormalize(lastPosObject - lastPosDevice));
lastDeviceObjectRot = cMul(cTrans(lastRotDevice), lastRotObject);
tooth->setHapticEnabled(true, true);
tool->setShowEnabled(true, true);
drill->setShowEnabled(true, true);
}
else
{
tool->setShowEnabled(false, true);
drill->setShowEnabled(false, true);
cMatrix3d rotDevice01 = cMul(cTrans(lastRotDevice), rot);
cMatrix3d newRot = cMul(rot, lastDeviceObjectRot);
cVector3d newPos = cAdd(pos, cMul(rot, lastDeviceObjectPos));
tooth->setPos(newPos);
tooth->setRot(newRot);
world->computeGlobalPositions(true);
tooth->setHapticEnabled(false, true);
}
// compute global reference frames for each object
world->computeGlobalPositions(true);
}
// exit haptics thread
simulationFinished = true;
}
void CQTOpenGLUserFunctions::DrawCylinder(Real f_radius,
Real f_height,
const CVector3& c_center_offset,
const CColor& c_color,
const CQuaternion& c_orientation,
GLuint un_vertices) {
/* Draw top circle*/
CVector3 cCirclePos(0.0f, 0.0f, f_height * 0.5f);
cCirclePos.Rotate(c_orientation);
cCirclePos += c_center_offset;
DrawCircle(f_radius,
cCirclePos,
c_color,
true,
c_orientation,
un_vertices);
/* Draw bottom circle*/
cCirclePos.Set(0.0f, 0.0f, -f_height * 0.5f);
cCirclePos.Rotate(c_orientation);
cCirclePos += c_center_offset;
DrawCircle(f_radius,
cCirclePos,
c_color,
true,
c_orientation,
un_vertices);
/* Side surface */
CVector3 cVertex1(f_radius, 0.0f, f_height * 0.5f);
CVector3 cVertex2(f_radius, 0.0f, -f_height * 0.5f);
CRadians cAngle(CRadians::TWO_PI / un_vertices);
/* Compute the quaternion defining the rotation of the vertices used to draw the side surface. */
CQuaternion cVertexRotation;
CVector3 cVertexRotationAxis(0.0f, 0.0f, 1.0f);
cVertexRotationAxis.Rotate(c_orientation);
cVertexRotation.FromAngleAxis(cAngle, cVertexRotationAxis);
glDisable(GL_LIGHTING);
glColor3ub(c_color.GetRed(),
c_color.GetGreen(),
c_color.GetBlue());
glBegin(GL_QUAD_STRIP);
/* Compute the normal direction of the starting edge. */
CVector3 cNormalDirection(cVertex1.GetX(), cVertex1.GetY(), 0.0f);
cNormalDirection.Rotate(c_orientation);
glNormal3f(cNormalDirection.GetX(),
cNormalDirection.GetY(),
cNormalDirection.GetZ());
/* Rotate the endpoints of the first edge.*/
cVertex1.Rotate(c_orientation);
cVertex2.Rotate(c_orientation);
for(GLuint i = 0; i <= un_vertices; i++) {
glVertex3f(cVertex1.GetX() + c_center_offset.GetX(),
c_center_offset.GetY() + cVertex1.GetY(),
c_center_offset.GetZ() + cVertex1.GetZ() );
glVertex3f(cVertex2.GetX() + c_center_offset.GetX(),
c_center_offset.GetY() + cVertex2.GetY(),
c_center_offset.GetZ() + cVertex2.GetZ() );
/* Rotate the vertices and the normal direction, set the new normal. */
cVertex1.Rotate(cVertexRotation);
cVertex2.Rotate(cVertexRotation);
cNormalDirection.Rotate(cVertexRotation);
glNormal3f(cNormalDirection.GetX(),
cNormalDirection.GetY(),
cNormalDirection.GetZ());
}
glEnd();
glEnable(GL_LIGHTING);
}
//===========================================================================
void cGELMesh::connectVerticesToSkeleton(bool a_connectToNodesOnly)
{
// get number of vertices
int numVertices = m_gelVertices.size();
// for each deformable vertex we search for the nearest sphere or link
for (int i=0; i<numVertices; i++)
{
// get current deformable vertex
cGELVertex* curVertex = &m_gelVertices[i];
// get current vertex position
cVector3d pos = curVertex->m_vertex->getPos();
// initialize constant
double min_distance = 99999999999999999.0;
cGELSkeletonNode* nearest_node = NULL;
cGELSkeletonLink* nearest_link = NULL;
// search for the nearest node
list<cGELSkeletonNode*>::iterator itr;
for(itr = m_nodes.begin(); itr != m_nodes.end(); ++itr)
{
cGELSkeletonNode* nextNode = *itr;
double distance = cDistance(pos, nextNode->m_pos);
if (distance < min_distance)
{
min_distance = distance;
nearest_node = nextNode;
nearest_link = NULL;
}
}
// search for the nearest link if any
if (!a_connectToNodesOnly)
{
list<cGELSkeletonLink*>::iterator j;
for(j = m_links.begin(); j != m_links.end(); ++j)
{
cGELSkeletonLink* nextLink = *j;
double angle0 = cAngle(nextLink->m_wLink01, cSub(pos, nextLink->m_node0->m_pos));
double angle1 = cAngle(nextLink->m_wLink10, cSub(pos, nextLink->m_node1->m_pos));
if ((angle0 < (CHAI_PI / 2.0)) && (angle1 < (CHAI_PI / 2.0)))
{
cVector3d p = cProjectPointOnLine(pos,
nextLink->m_node0->m_pos,
nextLink->m_wLink01);
double distance = cDistance(pos, p);
if (distance < min_distance)
{
min_distance = distance;
nearest_node = NULL;
nearest_link = nextLink;
}
}
}
}
// attach vertex to nearest node if it exists
if (nearest_node != NULL)
{
curVertex->m_node = nearest_node;
curVertex->m_link = NULL;
cVector3d posRel = cSub(pos, nearest_node->m_pos);
curVertex->m_massParticle->m_pos = cMul(cTrans(nearest_node->m_rot), posRel);
}
// attach vertex to nearest link if it exists
else if (nearest_link != NULL)
{
curVertex->m_node = NULL;
curVertex->m_link = nearest_link;
cMatrix3d rot;
rot.setCol( nearest_link->m_A0,
nearest_link->m_B0,
nearest_link->m_wLink01);
cVector3d posRel = cSub(pos, nearest_link->m_node0->m_pos);
curVertex->m_massParticle->m_pos = cMul(cInv(rot), posRel);
}
}
}
