void HapticLoop(void* param)
{
// simulation in now ON
Crecord_playerApp* app = (Crecord_playerApp*)(param);
// read position from haptic device
app->tool->updatePose();
// compute forces
app->tool->computeForces();
// get last interaction force in global coordinate frame
app->m_interactionForce = cMul(cTrans(app->object->getRot()), cSub(app->tool->m_lastComputedGlobalForce, app->object->getPos()));
app->tool->applyForces();
// figure out if we're touching the record
cProxyPointForceAlgo * algo = app->tool->getProxy();
if (algo->getContactObject() == app->m_recordMesh)
{
if (!app->m_inContact)
{
app->m_inContact = true;
app->m_RFDInitialAngle = app->m_rotPos - app->m_lastGoodPosition*CHAI_PI/180;
}
app->animateObject(app->m_interactionForce);
}
else
{
app->animateObject(cVector3d(0.0, 0.0, 0.0));
app->m_inContact = false;
}
}
int main()
{
char s[100];
double a, b;
struct complex C1, C2;
printf("Введите комплексное число в формате a, b: ");
scanf("%lf%lf", &a, &b);
C1 = cRead(a, b);
cPrint(C1);
printf("Введите комплексное число в формате a,b: ");
scanf("%lf%lf", &a, &b);
C2 = cRead(a,b);
cPrint(C2);
printf("РЎСѓРјРјР°: ");
cPrint(cAdd(C1, C2));
printf("Разность: ");
cPrint(cSub(C1, C2));
printf("Произведение: ");
cPrint(cMul(C1, C2));
printf("Частное: ");
cPrint(cDiv(C1, C2));
printf("Модуль 1-ого: %f: \n", cAbs(C1));
printf("Аргумент 1-ого: %f: \n", cArg(C1));
printf("Сопряжённое 1-ого: "); cPrint(cConj(C1));
printf("Re 1-РѕРіРѕ: %f: \n", cReal(C1));
printf("Im 1-РѕРіРѕ: %f: \n", cImag(C1));
return 0;
}
//===========================================================================
bool cEffectStickSlip::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
// check if history for this IDN exists
if (a_toolID < CHAI_EFFECT_MAX_IDN)
{
if (m_parent->m_interactionInside)
{
// check if a recent valid point has been stored previously
if (!m_history[a_toolID].m_valid)
{
m_history[a_toolID].m_currentStickPosition = a_toolPos;
m_history[a_toolID].m_valid = true;
}
// read parameters for stick and slip model
double stiffness = m_parent->m_material.getStickSlipStiffness();
double forceMax = m_parent->m_material.getStickSlipForceMax();
// compute current force between last stick position and current tool position
double distance = cDistance(a_toolPos, m_history[a_toolID].m_currentStickPosition);
double forceMag = distance * stiffness;
if (forceMag > 0)
{
// if force above threshold, slip...
if (forceMag > forceMax)
{
m_history[a_toolID].m_currentStickPosition = a_toolPos;
a_reactionForce.zero();
}
// ...otherwise stick
else
{
a_reactionForce = (forceMag / distance) * cSub(m_history[a_toolID].m_currentStickPosition, a_toolPos);
}
}
else
{
a_reactionForce.zero();
}
return (true);
}
else
{
// the tool is located outside the object, so zero reaction force
m_history[a_toolID].m_valid = false;
a_reactionForce.zero();
return (false);
}
}
else
{
a_reactionForce.zero();
return (false);
}
}
//==============================================================================
void cShapeTorus::computeLocalInteraction(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int a_IDN)
{
cVector3d toolProjection = a_toolPos;
toolProjection.z(0.0);
m_interactionNormal.set(0,0,1);
// search for the nearest point on the torus medial axis
if (a_toolPos.lengthsq() > C_SMALL)
{
cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(toolProjection));
// compute eventual penetration of tool inside the torus
cVector3d vectTorusTool = cSub(a_toolPos, pointAxisTorus);
double distance = vectTorusTool.length();
// normal
if (distance > 0.0)
{
m_interactionNormal = vectTorusTool;
m_interactionNormal.normalize();
}
// tool is located inside the torus
if ((distance < m_innerRadius) && (distance > 0.001))
{
m_interactionInside = true;
}
// tool is located outside the torus
else
{
m_interactionInside = false;
}
// compute surface point
double dist = vectTorusTool.length();
if (dist > 0)
{
vectTorusTool.mul(1/dist);
}
vectTorusTool.mul(m_innerRadius);
pointAxisTorus.addr(vectTorusTool, m_interactionPoint);
}
else
{
m_interactionInside = false;
m_interactionPoint = a_toolPos;
}
}
//===========================================================================
bool cEffectSurface::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
if (m_parent->m_interactionInside)
{
// the tool is located inside the object,
// we compute a reaction force using Hooke's law
double stiffness = m_parent->m_material.getStiffness();
a_reactionForce = cMul(stiffness, cSub(m_parent->m_interactionProjectedPoint, a_toolPos));
return (true);
}
else
{
// the tool is located outside the object, so zero reaction force
a_reactionForce.zero();
return (false);
}
}
//===========================================================================
cCollisionSpheresLine::cCollisionSpheresLine(cVector3d& a_segmentPointA,
cVector3d& a_segmentPointB)
{
// calculate the center of the line segment
m_center = cAdd(a_segmentPointA, a_segmentPointB);
m_center.x *= 0.5;
m_center.y *= 0.5;
m_center.z *= 0.5;
// calculate the radius of the bounding sphere as the distance from the
// center of the segment (calculated above) to an endpoint
cVector3d rad = cSub(m_center, a_segmentPointA);
m_radius = sqrt(rad.x*rad.x + rad.y*rad.y + rad.z*rad.z);
// set origin and direction of the line segment; i.e., redefine the segment
// as a ray from the first endpoint (presumably the proxy position when
// the collision detection is being used with the proxy force algorithm) to
// the second endpoint (presumably the goal position)
m_segmentPointA = a_segmentPointA;
a_segmentPointB.subr(a_segmentPointA, m_dir);
}
//===========================================================================
cVector3d cShapeTorus::computeLocalForce(const cVector3d& a_localPosition)
{
// In the following we compute the reaction forces between the tool and the
// sphere.
cVector3d localForce;
// project pointer on torus plane (z=0)
cVector3d fingerProjection = a_localPosition;
fingerProjection.z = 0;
// search for the nearest point on the torus medial axis
if (a_localPosition.lengthsq() > CHAI_SMALL)
{
cVector3d pointAxisTorus = cMul(m_outerRadius, cNormalize(fingerProjection));
// compute eventual penetration of finger inside the torus
cVector3d vectTorusFinger = cSub(a_localPosition, pointAxisTorus);
double distance = vectTorusFinger.length();
// finger inside torus, compute forces
if ((distance < m_innerRadius) && (distance > 0.001))
{
localForce = cMul((m_innerRadius - distance) * (m_material.getStiffness()), cNormalize(vectTorusFinger));
}
// finger is outside torus
else
{
localForce.zero();
}
}
else
{
localForce.zero();
}
return (localForce);
}
//===========================================================================
void cCollisionSpheresEdge::initialize(cCollisionSpheresPoint *a_a, cCollisionSpheresPoint *a_b)
{
// set the endpoints of the new edge
m_end[0] = a_a;
m_end[1] = a_b;
// insert the edge into the edge maps of both endpoints
m_end[0]->m_edgeMap.insert(PtEmap::value_type(m_end[1], this));
m_end[1]->m_edgeMap.insert(PtEmap::value_type(m_end[0], this));
// calculate the vector between the endpoints
m_d = cSub((*m_end[1]).m_pos, (*m_end[0]).m_pos);
// calculate the squared distance of the edge
m_D = m_d.x*m_d.x + m_d.y*m_d.y + m_d.z*m_d.z;
// calculate the center of the edge
double lambda = 0.5;
m_center.x = (*m_end[0]).m_pos.x + lambda*((*m_end[1]).m_pos.x - (*m_end[0]).m_pos.x);
m_center.y = (*m_end[0]).m_pos.y + lambda*((*m_end[1]).m_pos.y - (*m_end[0]).m_pos.y);
m_center.z = (*m_end[0]).m_pos.z + lambda*((*m_end[1]).m_pos.z - (*m_end[0]).m_pos.z);
}
//===========================================================================
bool cEffectPotentialField::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
if (m_parent->m_interactionInside)
{
// the tool is located inside the object, so zero reaction force
a_reactionForce.zero();
return (false);
}
else
{
// the tool is located outside the object,
// we compute a reaction force using Hooke's law
double stiffness = m_parent->m_material->getStiffness();
double maxForce = m_parent->m_material->getMagnetMaxForce();
cVector3d force = cMul(stiffness, cSub(m_parent->m_interactionProjectedPoint, a_toolPos));
if (maxForce > 0.0)
{
double ratio = force.length() / maxForce;
if (ratio > 1.0)
{
force = (1.0 / ratio) * force;
}
}
else
{
force.zero();
}
a_reactionForce = force;
return (true);
}
}
bool createSkeletonMesh(cGELMesh *a_object, char *a_filename, char *a_filenameHighRes)
{
a_object->m_useSkeletonModel = true;
a_object->m_useMassParticleModel = false;
a_object->loadFromFile(a_filenameHighRes);
cGELMesh* model = new cGELMesh(world);
tetgenio input;
if (input.load_off(a_filename))
{
// use TetGen to tetrahedralize our mesh
tetgenio output;
tetrahedralize(TETGEN_SWITCHES, &input, &output);
// create a vertex in the object for each point of the result
for (int p = 0, pi = 0; p < output.numberofpoints; ++p, pi += 3)
{
cVector3d point;
point.x = output.pointlist[pi+0];
point.y = output.pointlist[pi+1];
point.z = output.pointlist[pi+2];
model->newVertex(point);
}
// create a triangle for each face on the surface
for (int t = 0, ti = 0; t < output.numberoftrifaces; ++t, ti += 3)
{
cVector3d p[3];
int vi[3];
for (int i = 0; i < 3; ++i) {
int tc = output.trifacelist[ti+i];
vi[i] = tc;
int pi = tc*3;
p[i].x = output.pointlist[pi+0];
p[i].y = output.pointlist[pi+1];
p[i].z = output.pointlist[pi+2];
}
//unsigned int index = a_object->newTriangle(p[1], p[0], p[2]);
model->newTriangle(vi[1], vi[0], vi[2]);
}
// find out exactly which vertices are on the inside and outside
set<int> inside, outside;
for (int t = 0; t < output.numberoftrifaces * 3; ++t)
{
outside.insert(output.trifacelist[t]);
}
for (int p = 0; p < output.numberofpoints; ++p)
{
if (outside.find(p) == outside.end())
inside.insert(p);
}
model->computeAllNormals();
// compute a boundary box
model->computeBoundaryBox(true);
// get dimensions of object
double size = cSub(model->getBoundaryMax(), model->getBoundaryMin()).length();
// resize object to screen
if (size > 0)
{
model->scale( 1.5 / size);
a_object->scale( 1.5 / size);
}
// setup default values for nodes
cGELSkeletonNode::default_radius = 0.05;
cGELSkeletonNode::default_kDampingPos = 0.3;
cGELSkeletonNode::default_kDampingRot = 0.1;
cGELSkeletonNode::default_mass = 0.002; // [kg]
cGELSkeletonNode::default_showFrame = false;
cGELSkeletonNode::default_color.set(1.0, 0.6, 0.6);
cGELSkeletonNode::default_useGravity = true;
cGELSkeletonNode::default_gravity.set(0.00, 0.00, -3.45);
radius = cGELSkeletonNode::default_radius;
a_object->buildVertices();
model->buildVertices();
vector<cGELSkeletonNode*> nodes;
int i=0;
for (set<int>::iterator it = inside.begin(); it != inside.end(); ++it)
{
cGELSkeletonNode* newNode = new cGELSkeletonNode();
a_object->m_nodes.push_front(newNode);
cVertex* vertex = model->getVertex(*it);
newNode->m_pos = vertex->getPos();
newNode->m_rot.identity();
newNode->m_radius = 0.1;
newNode->m_fixed = false;
vertex->m_tag = i;
i++;
nodes.push_back(newNode);
}
// get all the edges of our tetrahedra
set< pair<int,int> > springs;
for (int t = 0, ti = 0; t < output.numberoftetrahedra; ++t, ti += 4)
{
// store each edge of the tetrahedron as a pair of indices
for (int i = 0; i < 4; ++i) {
int v0 = output.tetrahedronlist[ti+i];
for (int j = i+1; j < 4; ++j) {
int v1 = output.tetrahedronlist[ti+j];
// connect only if both points are inside
if (inside.find(v0) != inside.end() && inside.find(v1) != inside.end())
springs.insert(pair<int,int>(min(v0,v1), max(v0,v1)));
}
}
}
// setup default values for links
cGELSkeletonLink::default_kSpringElongation = 100.0; // [N/m]
cGELSkeletonLink::default_kSpringFlexion = 0.1; // [Nm/RAD]
cGELSkeletonLink::default_kSpringTorsion = 0.1; // [Nm/RAD]
cGELSkeletonLink::default_color.set(0.2, 0.2, 1.0);
for (set< pair<int,int> >::iterator it = springs.begin(); it != springs.end(); ++it)
{
cVertex* v0 = model->getVertex(it->first);
cVertex* v1 = model->getVertex(it->second);
cGELSkeletonNode* n0 = nodes[v0->m_tag];
cGELSkeletonNode* n1 = nodes[v1->m_tag];
cGELSkeletonLink* newLink = new cGELSkeletonLink(n0, n1);
a_object->m_links.push_front(newLink);
}
a_object->connectVerticesToSkeleton(false);
cMaterial mat;
mat.m_ambient.set(0.7, 0.7, 0.7);
mat.m_diffuse.set(0.8, 0.8, 0.8);
mat.m_specular.set(0.0, 0.0, 0.0);
a_object->setMaterial(mat, true);
return (true);
}
return (false);
}
bool createTetGenMesh(cGELMesh *a_object, char *a_filename, char *a_filenameHighRes)
{
a_object->m_useSkeletonModel = false;
a_object->m_useMassParticleModel = true;
cGELMesh* model = new cGELMesh(world);
model->loadFromFile(a_filenameHighRes);
tetgenio input;
if (input.load_off(a_filename))
{
// use TetGen to tetrahedralize our mesh
tetgenio output;
tetrahedralize(TETGEN_SWITCHES0, &input, &output);
// create a vertex in the object for each point of the result
for (int p = 0, pi = 0; p < output.numberofpoints; ++p, pi += 3)
{
cVector3d point;
point.x = output.pointlist[pi+0];
point.y = output.pointlist[pi+1];
point.z = output.pointlist[pi+2];
a_object->newVertex(point);
}
// create a triangle for each face on the surface
set<int> outside;
for (int t = 0, ti = 0; t < output.numberoftrifaces; ++t, ti += 3)
{
cVector3d p[3];
int vi[3];
for (int i = 0; i < 3; ++i) {
int tc = output.trifacelist[ti+i];
outside.insert(tc);
vi[i] = tc;
int pi = tc*3;
p[i].x = output.pointlist[pi+0];
p[i].y = output.pointlist[pi+1];
p[i].z = output.pointlist[pi+2];
}
//unsigned int index = a_object->newTriangle(p[1], p[0], p[2]);
a_object->newTriangle(vi[1], vi[0], vi[2]);
}
a_object->computeAllNormals();
// compute a boundary box
a_object->computeBoundaryBox(true);
// get dimensions of object
double size = cSub(a_object->getBoundaryMax(), a_object->getBoundaryMin()).length();
// resize object to screen
if (size > 0)
{
model->scale(1.5 / size);
a_object->scale(1.5 / size);
}
// build dynamic vertices
cGELMassParticle::default_mass = 0.002;
cGELMassParticle::default_kDampingPos = 4.0;
cGELMassParticle::default_gravity.set(0,0,-0.1);
a_object->buildVertices();
// get all the edges of our tetrahedra
set< pair<int,int> > springs;
for (int t = 0, ti = 0; t < output.numberoftetrahedra; ++t, ti += 4)
{
// store each edge of the tetrahedron as a pair of indices
for (int i = 0; i < 4; ++i) {
int v0 = output.tetrahedronlist[ti+i];
for (int j = i+1; j < 4; ++j) {
int v1 = output.tetrahedronlist[ti+j];
springs.insert(pair<int,int>(min(v0,v1), max(v0,v1)));
}
}
}
// create a spring on each tetrahedral edge we found in the output
cGELLinearSpring::default_kSpringElongation = 40.0; // 0.55;
for (set< pair<int,int> >::iterator it = springs.begin(); it != springs.end(); ++it)
{
cVertex* v0 = a_object->getVertex(it->first);
cVertex* v1 = a_object->getVertex(it->second);
cGELMassParticle* m0 = a_object->m_gelVertices[v0->m_tag].m_massParticle;
cGELMassParticle* m1 = a_object->m_gelVertices[v1->m_tag].m_massParticle;
cGELLinearSpring* spring = new cGELLinearSpring(m0, m1);
a_object->m_linearSprings.push_back(spring);
}
// extract texture
int numModelV = model->getNumVertices(true);
for (set<int>::iterator it = outside.begin(); it != outside.end(); ++it)
{
cVertex *v = a_object->getVertex(*it);
cVertex *t = 0;
double closest = 1e10;
for (int j = 0; j < numModelV; ++j) {
cVertex *test = model->getVertex(j, true);
double d = cDistanceSq(v->getPos(), test->getPos());
if (d < closest) {
closest = d;
t = test;
}
}
v->setTexCoord(t->getTexCoord());
}
cMesh* mesh = (cMesh*)model->getChild(0);
mesh->m_texture->setWrapMode(GL_CLAMP, GL_CLAMP);
a_object->setTexture(mesh->m_texture, true);
a_object->setUseTexture(true, true);
cMaterial mat;
mat.m_ambient.set(0.7, 0.7, 0.7);
mat.m_diffuse.set(0.8, 0.8, 0.8);
mat.m_specular.set(0.0, 0.0, 0.0);
a_object->setMaterial(mat, true);
return (true);
}
return (false);
}
void updateHaptics(void)
{
// reset clock
cPrecisionClock clock;
clock.reset();
// main haptic simulation loop
while(simulationRunning)
{
/////////////////////////////////////////////////////////////////////
// SIMULATION TIME
/////////////////////////////////////////////////////////////////////
// stop the simulation clock
clock.stop();
// read the time increment in seconds
double timeInterval = clock.getCurrentTimeSeconds();
// restart the simulation clock
clock.reset();
clock.start();
// update frequency counter
frequencyCounter.signal(1);
/////////////////////////////////////////////////////////////////////
// HAPTIC FORCE COMPUTATION
/////////////////////////////////////////////////////////////////////
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
/////////////////////////////////////////////////////////////////////
// HAPTIC SIMULATION
/////////////////////////////////////////////////////////////////////
// get position of cursor in global coordinates
cVector3d toolPos = tool->getDeviceGlobalPos();
// get position of object in global coordinates
cVector3d objectPos = object->getGlobalPos();
// compute a vector from the center of mass of the object (point of rotation) to the tool
cVector3d v = cSub(toolPos, objectPos);
// compute angular acceleration based on the interaction forces
// between the tool and the object
cVector3d angAcc(0,0,0);
if (v.length() > 0.0)
{
// get the last force applied to the cursor in global coordinates
// we negate the result to obtain the opposite force that is applied on the
// object
cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);
// compute the effective force that contributes to rotating the object.
cVector3d force = toolForce - cProject(toolForce, v);
// compute the resulting torque
cVector3d torque = cMul(v.length(), cCross( cNormalize(v), force));
// update rotational acceleration
const double INERTIA = 0.4;
angAcc = (1.0 / INERTIA) * torque;
}
// update rotational velocity
angVel.add(timeInterval * angAcc);
// set a threshold on the rotational velocity term
const double MAX_ANG_VEL = 10.0;
double vel = angVel.length();
if (vel > MAX_ANG_VEL)
{
angAcc.mul(MAX_ANG_VEL / vel);
}
// add some damping too
const double DAMPING = 0.1;
angVel.mul(1.0 - DAMPING * timeInterval);
// if user switch is pressed, set velocity to zero
if (tool->getUserSwitch(0) == 1)
{
angVel.zero();
}
// compute the next rotation configuration of the object
if (angVel.length() > C_SMALL)
{
object->rotateAboutGlobalAxisRad(cNormalize(angVel), timeInterval * angVel.length());
}
}
// exit haptics thread
simulationFinished = true;
}
//===========================================================================
bool cEffectMagnet::computeForce(const cVector3d& a_toolPos,
const cVector3d& a_toolVel,
const unsigned int& a_toolID,
cVector3d& a_reactionForce)
{
// compute distance from object to tool
double distance = cDistance(a_toolPos, m_parent->m_interactionProjectedPoint);
// get parameters of magnet
double magnetMaxForce = m_parent->m_material.getMagnetMaxForce();
double magnetMaxDistance = m_parent->m_material.getMagnetMaxDistance();
double stiffness = m_parent->m_material.getStiffness();
double forceMagnitude = 0;
if ((distance > 0) && (distance < magnetMaxDistance) && (stiffness > 0))
{
double limitLinearModel = magnetMaxForce / stiffness;
cClamp(limitLinearModel, 0.0, 0.5 * distance);
if (distance < limitLinearModel)
{
// apply local linear model near magnet
forceMagnitude = stiffness * distance;
}
else
{
// compute quadratic model
cMatrix3d sys;
sys.m[0][0] = limitLinearModel * limitLinearModel;
sys.m[0][1] = limitLinearModel;
sys.m[0][2] = 1.0;
sys.m[1][0] = magnetMaxDistance * magnetMaxDistance;
sys.m[1][1] = magnetMaxDistance;
sys.m[1][2] = 1.0;
sys.m[2][0] = 2.0 * limitLinearModel;
sys.m[2][1] = 1.0;
sys.m[2][2] = 0.0;
sys.invert();
cVector3d param;
sys.mulr(cVector3d(magnetMaxForce, 0.0, -1.0), param);
// apply quadratic model
double val = distance - limitLinearModel;
forceMagnitude = param.x * val * val + param.y * val + param.z;
}
// compute magnetic force
a_reactionForce = cMul(forceMagnitude, cNormalize(cSub(m_parent->m_interactionProjectedPoint, a_toolPos)));
// add damping component
double viscosity = m_parent->m_material.getViscosity();
cVector3d viscousForce = cMul(-viscosity, a_toolVel);
a_reactionForce.add(viscousForce);
return (true);
}
else
{
// the tool is located outside the magnet zone
a_reactionForce.zero();
return (false);
}
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI3D\n");
printf ("Demo: 22-chrome\n");
printf ("Copyright 2003-2012\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[1] - Texture ON\n");
printf ("[2] - Texture OFF\n");
printf ("[3] - Wireframe ON\n");
printf ("[4] - Wireframe OFF\n");
printf ("[5] - Normals ON\n");
printf ("[6] - Normals OFF\n");
printf ("[x] - Exit application\n");
printf ("\n\n>\r");
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
//-----------------------------------------------------------------------
// WORLD - CAMERA - LIGHTING
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// set the background color of the environment
world->m_backgroundColor.setBlack();
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
camera->set( cVector3d (3.0, 0.0, 0.6), // camera position (eye)
cVector3d (0.0, 0.0, 0.0), // lookat position (target)
cVector3d (0.0, 0.0, 1.0)); // direction of the (up) vector
// set the near and far clipping planes of the camera
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.01, 10.0);
// Enable shadow casting
camera->setUseShadowCasting(true);
// create a light source
light = new cSpotLight(world);
// attach light to camera
camera->addChild(light);
// enable light source
light->setEnabled(true);
// position the light source
light->setLocalPos( 0.0, 0.5, 0.0);
// define the direction of the light beam
light->setDir(-3.0,-0.5, 0.0);
// enable this light source to generate shadows
light->setShadowMapEnabled(true);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info = hapticDevice->getSpecifications();
// if the haptic devices carries a gripper, enable it to behave like a user switch
hapticDevice->setEnableGripperUserSwitch(true);
// create a 3D tool and add it to the world
tool = new cToolCursor(world);
world->addChild(tool);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// initialize tool by connecting to haptic device
tool->start();
// map the physical workspace of the haptic device to a larger virtual workspace.
tool->setWorkspaceRadius(1.0);
// define the radius of the tool (sphere)
double toolRadius = 0.04;
// define a radius for the tool
tool->setRadius(toolRadius);
// hide the device sphere. only show proxy.
tool->setShowContactPoints(true, false);
// enable if objects in the scene are going to rotate of translate
// or possibly collide against the tool. If the environment
// is entirely static, you can set this parameter to "false"
tool->enableDynamicObjects(true);
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
double workspaceScaleFactor = tool->getWorkspaceScaleFactor();
// define a maximum stiffness that can be handled by the current
// haptic device. The value is scaled to take into account the
// workspace scale factor
double stiffnessMax = info.m_maxLinearStiffness / workspaceScaleFactor;
//-----------------------------------------------------------------------
// CREATING OBJECT
//-----------------------------------------------------------------------
// create a virtual mesh
object = new cMultiMesh();
// add object to world
world->addChild(object);
// set the position of the object at the center of the world
object->setLocalPos(0.0, 0.0, 0.0);
// rotate the object 90 degrees
object->rotateAboutGlobalAxisDeg(cVector3d(0,0,1), 90);
// load an object file
bool fileload;
fileload = object->loadFromFile(RESOURCE_PATH("resources/models/face/face.obj"));
if (!fileload)
{
#if defined(_MSVC)
fileload = object->loadFromFile("../../../bin/resources/models/face/face.obj");
#endif
}
if (!fileload)
{
printf("Error - 3D Model failed to load correctly.\n");
close();
return (-1);
}
// compute a boundary box
object->computeBoundaryBox(true);
// get dimensions of object
double size = cSub(object->getBoundaryMax(), object->getBoundaryMin()).length();
// resize object to screen
if (size > 0)
{
object->scale( 2.0 * tool->getWorkspaceRadius() / size);
}
// compute collision detection algorithm
object->createAABBCollisionDetector(toolRadius);
cMaterial mat;
mat.setRenderTriangles(true, true);
object->setMaterial(mat);
// define some environmental texture mapping
cTexture2d* texture = new cTexture2d();
// load texture file
fileload = texture->loadFromFile(RESOURCE_PATH("resources/images/chrome.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = texture->loadFromFile("../../../bin/resources/images/chrome.bmp");
#endif
}
if (!fileload)
{
printf("Error - Texture image failed to load correctly.\n");
close();
return (-1);
}
// enable spherical mapping
texture->setSphericalMappingEnabled(true);
// assign texture to object
object->setTexture(texture, true);
// enable texture mapping
object->setUseTexture(true, true);
// disable culling
object->setUseCulling(false, true);
// define a default stiffness for the object
object->setStiffness(stiffnessMax, true);
// define some haptic friction properties
object->setFriction(0.1, 0.2, true);
//-----------------------------------------------------------------------
// WIDGETS
//-----------------------------------------------------------------------
// create a font
cFont *font = NEW_CFONTCALIBRI20();
// create a label to display the haptic rate of the simulation
labelHapticRate = new cLabel(font);
labelHapticRate->m_fontColor.setWhite();
camera->m_frontLayer->addChild(labelHapticRate);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// simulation in now running!
simulationRunning = true;
// initialize GLUT
glutInit(&argc, argv);
// retrieve the resolution of the computer display and estimate the position
// of the GLUT window so that it is located at the center of the screen
int screenW = glutGet(GLUT_SCREEN_WIDTH);
int screenH = glutGet(GLUT_SCREEN_HEIGHT);
int windowW = 0.7 * screenH;
int windowH = 0.7 * screenH;
int windowPosX = (screenW - windowH) / 2;
int windowPosY = (screenH - windowW) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(windowW, windowH);
if (USE_STEREO_DISPLAY)
{
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE | GLUT_STEREO);
camera->setUseStereo(true);
}
else
{
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
camera->setUseStereo(false);
}
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->start(updateHaptics, CTHREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutTimerFunc(30, graphicsTimer, 0);
glutMainLoop();
// close everything
close();
// exit
return (0);
}
/***
Enable or disable haptics; called when the user clicks
the enable/disable haptics button. The "enable" parameter
is one of :
#define TOGGLE_HAPTICS_TOGGLE -1
#define TOGGLE_HAPTICS_DISABLE 0
#define TOGGLE_HAPTICS_ENABLE 1
***/
void Cmesh_mesh_collisionsApp::toggle_haptics(int enable) {
if (enable == TOGGLE_HAPTICS_TOGGLE) {
if (haptics_enabled) toggle_haptics(TOGGLE_HAPTICS_DISABLE);
else toggle_haptics(TOGGLE_HAPTICS_ENABLE);
}
else if (enable == TOGGLE_HAPTICS_ENABLE) {
if (haptics_enabled) return;
haptics_enabled = 1;
// create a phantom tool with its graphical representation
//
// Use device zero, and use either the gstEffect or direct
// i/o communication mode, depending on the USE_PHANTOM_DIRECT_IO
// constant
if (tool == 0) {
// Create a new tool with this mesh
tool = new cMeshTool(world, 0, true);
world->addChild(tool);
// Load a gear mesh from a .3DS file
tool_object = new cMesh(world);
tool_object->loadFromFile("resources\\models\\small_gear.3ds");
tool_object->computeGlobalPositions(false);
// Scale the object to fit nicely in our viewport
// compute size of object
tool_object->computeBoundaryBox(true);
cVector3d min = tool_object->getBoundaryMin();
cVector3d max = tool_object->getBoundaryMax();
// This is the "size" of the object
cVector3d span = cSub(max, min);
double size = cMax(span.x, cMax(span.y, span.z));
// We'll center all vertices, then multiply by this amount,
// to scale to the desired size.
double scaleFactor = 2.0 / size;
tool_object->scale(scaleFactor);
// Create a sphere tree bounding volume hierarchy for collision detection on this mesh
tool_object->createSphereTreeCollisionDetector(true, true);
// Use vertex colors so we can see which triangles collide
tool_object->useColors(true, true);
// Add the mesh object to the world
world->addChild(tool_object);
// Set the mesh for this tool
tool->setMesh(tool_object);
// Tell the tool to search for collisions with this mesh
tool->addCollisionMesh(object);
// Set up the device
tool->initialize();
// Set up a nice-looking workspace for the phantom so
// it fits nicely with our shape
tool->setWorkspace(2.0, 2.0, 2.0);
// Rotate the tool so its axes align with our opengl-like axes
tool->rotate(cVector3d(0,0,1), -90.0*3.14159/180.0);
tool->rotate(cVector3d(1,0,0), -90.0*3.14159/180.0);
tool->setRadius(0.05);
}
// I need to call this so the tool can update its internal
// transformations before performing collision detection, etc.
tool->computeGlobalPositions();
// Open communication with the device
tool->start();
// Enable forces
tool->setForcesON();
// Tell the proxy algorithm associated with this tool to enable its
// "dynamic mode", which allows interaction with moving objects
// The dynamic proxy is in a pretty beta state, so we turn it off for now...
// tool->getProxy()->enableDynamicProxy(1);
#ifdef USE_MM_TIMER_FOR_HAPTICS
// start the mm timer to run the haptic loop
timer.set(0,mesh_mesh_collisions_haptic_iteration,this);
#else
// start haptic thread
haptics_thread_running = 1;
DWORD thread_id;
::CreateThread(0, 0, (LPTHREAD_START_ROUTINE)(mesh_mesh_collisions_haptic_loop), this, 0, &thread_id);
// Boost thread and process priority
::SetThreadPriority(&thread_id, THREAD_PRIORITY_ABOVE_NORMAL);
//::SetPriorityClass(GetCurrentProcess(),ABOVE_NORMAL_PRIORITY_CLASS);
#endif
} // enabling
else if (enable == TOGGLE_HAPTICS_DISABLE) {
// Don't do anything if haptics are already off
if (haptics_enabled == 0) return;
// tell the haptic thread to quit
haptics_enabled = 0;
#ifdef USE_MM_TIMER_FOR_HAPTICS
timer.stop();
#else
// wait for the haptic thread to quit
while(haptics_thread_running) Sleep(1);
#endif
// Stop the haptic device...
tool->setForcesOFF();
tool->stop();
// SetPriorityClass(GetCurrentProcess(),NORMAL_PRIORITY_CLASS);
} // disabling
} // toggle_haptics()
int loadHeightMap()
{
// create a texture file
cTexture2D* newTexture = new cTexture2D();
world->addTexture(newTexture);
// texture 2D
bool fileload = newTexture->loadFromFile(RESOURCE_PATH("resources/images/map.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = newTexture->loadFromFile("../../../bin/resources/images/map.bmp");
#endif
}
if (!fileload)
{
printf("Error - Texture image failed to load correctly.\n");
close();
return (-1);
}
// get the size of the texture image (U and V)
int texSizeU = newTexture->m_image.getWidth();
int texSizeV = newTexture->m_image.getHeight();
// check size of image
if ((texSizeU < 1) || (texSizeV < 1)) { return (false); }
// we look for the largest side
int largestSide;
if (texSizeU > texSizeV)
{
largestSide = texSizeU;
}
else
{
largestSide = texSizeV;
}
// The largest side of the map has a length of 1.0
// we now compute the respective size for 1 pixel of the image in world space.
double size = 1.0 / (double)largestSide;
// we will create an triangle based object. For centering puposes we
// compute an offset for axis X and Y corresponding to the half size
// of the image map.
double offsetU = 0.5 * (double)texSizeU * size;
double offsetV = 0.5 * (double)texSizeV * size;
// For each pixel of the image, create a vertex
int u,v;
for (v=0; v<texSizeV; v++)
{
for (u=0; u<texSizeU; u++)
{
double px, py, tu, tv;
// compute the height of the vertex
cColorb color = newTexture->m_image.getPixelColor(u,v);
double height = 0.1 * ((double)color.getR() + (double)color.getG() + (double)color.getB()) / (3.0 * 255.0);
// compute the position of the vertex
px = size * (double)u - offsetU;
py = size * (double)v - offsetV;
// create new vertex
unsigned int index = object->newVertex(px, py, height);
cVertex* vertex = object->getVertex(index);
// compute texture coordinate
tu = (double)u / texSizeU;
tv = (double)v / texSizeV;
vertex->setTexCoord(tu, tv);
}
}
// Create a triangle based map using the above pixels
for (v=0; v<(texSizeV-1); v++)
{
for (u=0; u<(texSizeU-1); u++)
{
// get the indexing numbers of the next four vertices
unsigned int index00 = ((v + 0) * texSizeU) + (u + 0);
unsigned int index01 = ((v + 0) * texSizeU) + (u + 1);
unsigned int index10 = ((v + 1) * texSizeU) + (u + 0);
unsigned int index11 = ((v + 1) * texSizeU) + (u + 1);
// create two new triangles
object->newTriangle(index00, index01, index10);
object->newTriangle(index10, index01, index11);
}
}
// apply texture to object
object->setTexture(newTexture);
object->setUseTexture(true);
// compute normals
object->computeAllNormals(true);
// compute size of object
object->computeBoundaryBox(true);
cVector3d min = object->getBoundaryMin();
cVector3d max = object->getBoundaryMax();
// This is the "size" of the object
cVector3d span = cSub(max, min);
size = cMax(span.x, cMax(span.y, span.z));
// We'll center all vertices, then multiply by this amount,
// to scale to the desired size.
double scaleFactor = MESH_SCALE_SIZE / size;
object->scale(scaleFactor);
// compute size of object again
object->computeBoundaryBox(true);
// Build a collision-detector for this object, so
// the proxy will work nicely when haptics are enabled.
object->createAABBCollisionDetector(1.01 * proxyRadius, true, false);
// set size of frame
object->setFrameSize(0.2, true);
// set size of normals
object->setNormalsProperties(0.01, cColorf(1.0, 0.0, 0.0, 1.0), true);
// render graphically both sides of triangles
object->setUseCulling(false);
// update global position
object->computeGlobalPositions();
// success
return (0);
}
//===========================================================================
void cProxyPointForceAlgo::testFrictionAndMoveProxy(const cVector3d& a_goal,
const cVector3d& a_proxy,
cVector3d& a_normal,
cGenericObject* a_parent)
{
// check if friction is enabled
if (m_useFriction == false)
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
// Compute penetration depth; how far is the device "behind" the
// plane of the obstructing surface
cVector3d projectedGoal = cProjectPointOnPlane(m_deviceGlobalPos, a_proxy, a_normal);
double penetrationDepth = cSub(m_deviceGlobalPos,projectedGoal).length();
// Find the appropriate friction coefficient
// Our dynamic and static coefficients...
cMesh* parent_mesh = dynamic_cast<cMesh*>(a_parent);
// Right now we can only work with cMesh's
if (parent_mesh == NULL)
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
double mud = parent_mesh->m_material.getDynamicFriction();
double mus = parent_mesh->m_material.getStaticFriction();
// No friction; don't try to compute friction cones
if ((mud == 0) && (mus == 0))
{
m_nextBestProxyGlobalPos = a_goal;
return;
}
// The corresponding friction cone radii
double atmd = atan(mud);
double atms = atan(mus);
// Compute a vector from the device to the proxy, for computing
// the angle of the friction cone
cVector3d vDeviceProxy = cSub(a_proxy, m_deviceGlobalPos);
vDeviceProxy.normalize();
// Now compute the angle of the friction cone...
double theta = acos(vDeviceProxy.dot(a_normal));
// Manage the "slip-friction" state machine
// If the dynamic friction radius is for some reason larger than the
// static friction radius, always slip
if (mud > mus)
{
m_slipping = true;
}
// If we're slipping...
else if (m_slipping)
{
if (theta < (atmd * m_frictionDynHysteresisMultiplier))
{
m_slipping = false;
}
else
{
m_slipping = true;
}
}
// If we're not slipping...
else
{
if (theta > atms)
{
m_slipping = true;
}
else
{
m_slipping = false;
}
}
// The friction coefficient we're going to use...
double mu;
if (m_slipping) mu = mud;
else mu = mus;
// Calculate the friction radius as the absolute value of the penetration
// depth times the coefficient of friction
double frictionRadius = fabs(penetrationDepth * mu);
// Calculate the distance between the proxy position and the current
// goal position.
double r = a_proxy.distance(a_goal);
// If this distance is smaller than CHAI_SMALL, we consider the proxy
// to be at the same position as the goal, and we're done...
if (r < CHAI_SMALL)
{
m_nextBestProxyGlobalPos = a_proxy;
}
// If the proxy is outside the friction cone, update its position to
// be on the perimeter of the friction cone...
else if (r > frictionRadius)
{
m_nextBestProxyGlobalPos = cAdd(a_goal, cMul(frictionRadius/r, cSub(a_proxy, a_goal)));
}
// Otherwise, if the proxy is inside the friction cone, the proxy
// should not be moved (set next best position to current position)
else
{
m_nextBestProxyGlobalPos = a_proxy;
}
// We're done; record the fact that we're still touching an object...
return;
}
void hapticsLoop(void* a_pUserData)
{
// read the position of the haptic device
cursor->updatePose();
// compute forces between the cursor and the environment
cursor->computeForces();
// stop the simulation clock
g_clock.stop();
// read the time increment in seconds
double increment = g_clock.getCurrentTime() / 1000000.0;
// restart the simulation clock
g_clock.initialize();
g_clock.start();
// get position of cursor in global coordinates
cVector3d cursorPos = cursor->m_deviceGlobalPos;
// compute velocity of cursor;
timeCounter = timeCounter + increment;
if (timeCounter > 0.01)
{
cursorVel = (cursorPos - lastCursorPos) / timeCounter;
lastCursorPos = cursorPos;
timeCounter = 0;
}
// get position of torus in global coordinates
cVector3d objectPos = object->getGlobalPos();
// compute the velocity of the sphere at the contact point
cVector3d contactVel = cVector3d(0.0, 0.0, 0.0);
if (rotVelocity.length() > CHAI_SMALL)
{
cVector3d projection = cProjectPointOnLine(cursorPos, objectPos, rotVelocity);
cVector3d vpc = cursorPos - projection;
if (vpc.length() > CHAI_SMALL)
{
contactVel = vpc.length() * rotVelocity.length() * cNormalize(cCross(rotVelocity, vpc));
}
}
// get the last force applied to the cursor in global coordinates
cVector3d cursorForce = cursor->m_lastComputedGlobalForce;
// compute friction force
cVector3d friction = -40.0 * cursorForce.length() * cProjectPointOnPlane((cursorVel - contactVel), cVector3d(0.0, 0.0, 0.0), (cursorPos - objectPos));
// add friction force to cursor
cursor->m_lastComputedGlobalForce.add(friction);
// update rotational velocity
if (friction.length() > CHAI_SMALL)
{
rotVelocity.add( cMul(-10.0 * increment, cCross(cSub(cursorPos, objectPos), friction)));
}
// add some damping...
//rotVelocity.mul(1.0 - increment);
// compute the next rotation of the torus
if (rotVelocity.length() > CHAI_SMALL)
{
object->rotate(cNormalize(rotVelocity), increment * rotVelocity.length());
}
// send forces to haptic device
cursor->applyForces();
}
//===========================================================================
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);
}
}
}
//==============================================================================
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;
}
int main(int argc, char* argv[])
{
//-----------------------------------------------------------------------
// INITIALIZATION
//-----------------------------------------------------------------------
printf ("\n");
printf ("-----------------------------------\n");
printf ("CHAI 3D\n");
printf ("Demo: 25-cubic\n");
printf ("Copyright 2003-2009\n");
printf ("-----------------------------------\n");
printf ("\n\n");
printf ("Keyboard Options:\n\n");
printf ("[x] - Exit application\n");
printf ("\n\n");
// parse first arg to try and locate resources
resourceRoot = string(argv[0]).substr(0,string(argv[0]).find_last_of("/\\")+1);
//-----------------------------------------------------------------------
// 3D - SCENEGRAPH
//-----------------------------------------------------------------------
// create a new world.
world = new cWorld();
// set the background color of the environment
// the color is defined by its (R,G,B) components.
world->setBackgroundColor(1.0, 1.0, 1.0);
// create a camera and insert it into the virtual world
camera = new cCamera(world);
world->addChild(camera);
// position and oriente the camera
camera->set( cVector3d (3.0, 0.0, 0.0), // camera position (eye)
cVector3d (0.0, 0.0, 0.0), // lookat position (target)
cVector3d (0.0, 0.0, 1.0)); // direction of the "up" vector
// set the near and far clipping planes of the camera
// anything in front/behind these clipping planes will not be rendered
camera->setClippingPlanes(0.01, 10.0);
// create a light source and attach it to the camera
light = new cLight(world);
camera->addChild(light); // attach light to camera
light->setEnabled(true); // enable light source
light->setPos(cVector3d( 2.0, 0.5, 1.0)); // position the light source
light->setDir(cVector3d(-2.0, 0.5, 1.0)); // define the direction of the light beam
//-----------------------------------------------------------------------
// 2D - WIDGETS
//-----------------------------------------------------------------------
// create a 2D bitmap logo
logo = new cBitmap();
// add logo to the front plane
camera->m_front_2Dscene.addChild(logo);
// load a "chai3d" bitmap image file
bool fileload;
fileload = logo->m_image.loadFromFile(RESOURCE_PATH("resources/images/chai3d-w.bmp"));
if (!fileload)
{
#if defined(_MSVC)
fileload = logo->m_image.loadFromFile("../../../bin/resources/images/chai3d-w.bmp");
#endif
}
// position the logo at the bottom left of the screen (pixel coordinates)
logo->setPos(10, 10, 0);
// scale the logo along its horizontal and vertical axis
logo->setZoomHV(0.25, 0.25);
// here we replace all wite pixels (1,1,1) of the logo bitmap
// with transparent black pixels (1, 1, 1, 0). This allows us to make
// the background of the logo look transparent.
logo->m_image.replace(
cColorb(0xff, 0xff, 0xff), // original RGB color
cColorb(0xff, 0xff, 0xff, 0x00) // new RGBA color
);
// enable transparency
logo->enableTransparency(true);
//-----------------------------------------------------------------------
// HAPTIC DEVICES / TOOLS
//-----------------------------------------------------------------------
// create a haptic device handler
handler = new cHapticDeviceHandler();
// get access to the first available haptic device
cGenericHapticDevice* hapticDevice;
handler->getDevice(hapticDevice, 0);
// retrieve information about the current haptic device
cHapticDeviceInfo info;
if (hapticDevice)
{
info = hapticDevice->getSpecifications();
}
// create a 3D tool and add it to the world
tool = new cGeneric3dofPointer(world);
world->addChild(tool);
// connect the haptic device to the tool
tool->setHapticDevice(hapticDevice);
// initialize tool by connecting to haptic device
tool->start();
// map the physical workspace of the haptic device to a larger virtual workspace.
tool->setWorkspaceRadius(1.0);
// define a radius for the tool (graphical display)
tool->setRadius(0.05);
// hide the device sphere. only show proxy.
tool->m_deviceSphere->setShowEnabled(false);
// set the physical readius of the proxy.
proxyRadius = 0.05;
tool->m_proxyPointForceModel->setProxyRadius(proxyRadius);
tool->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;
// enable if objects in the scene are going to rotate of translate
// or possibly collide against the tool. If the environment
// is entirely static, you can set this parameter to "false"
tool->m_proxyPointForceModel->m_useDynamicProxy = true;
// read the scale factor between the physical workspace of the haptic
// device and the virtual workspace defined for the tool
double workspaceScaleFactor = tool->getWorkspaceScaleFactor();
// define a maximum stiffness that can be handled by the current
// haptic device. The value is scaled to take into account the
// workspace scale factor
double stiffnessMax = info.m_maxForceStiffness / workspaceScaleFactor;
//-----------------------------------------------------------------------
// COMPOSE THE VIRTUAL SCENE
//-----------------------------------------------------------------------
// create a virtual mesh
object = new cMesh(world);
// add object to world
world->addChild(object);
// set the position of the object at the center of the world
object->setPos(0.0, 0.0, 0.0);
/////////////////////////////////////////////////////////////////////////
// create a cube
/////////////////////////////////////////////////////////////////////////
const double HALFSIZE = 0.08;
// face -x
vertices[0][0] = object->newVertex(-HALFSIZE, HALFSIZE, -HALFSIZE);
vertices[0][1] = object->newVertex(-HALFSIZE, -HALFSIZE, -HALFSIZE);
vertices[0][2] = object->newVertex(-HALFSIZE, -HALFSIZE, HALFSIZE);
vertices[0][3] = object->newVertex(-HALFSIZE, HALFSIZE, HALFSIZE);
// face +x
vertices[1][0] = object->newVertex( HALFSIZE, -HALFSIZE, -HALFSIZE);
vertices[1][1] = object->newVertex( HALFSIZE, HALFSIZE, -HALFSIZE);
vertices[1][2] = object->newVertex( HALFSIZE, HALFSIZE, HALFSIZE);
vertices[1][3] = object->newVertex( HALFSIZE, -HALFSIZE, HALFSIZE);
// face -y
vertices[2][0] = object->newVertex(-HALFSIZE, -HALFSIZE, -HALFSIZE);
vertices[2][1] = object->newVertex( HALFSIZE, -HALFSIZE, -HALFSIZE);
vertices[2][2] = object->newVertex( HALFSIZE, -HALFSIZE, HALFSIZE);
vertices[2][3] = object->newVertex(-HALFSIZE, -HALFSIZE, HALFSIZE);
// face +y
vertices[3][0] = object->newVertex( HALFSIZE, HALFSIZE, -HALFSIZE);
vertices[3][1] = object->newVertex(-HALFSIZE, HALFSIZE, -HALFSIZE);
vertices[3][2] = object->newVertex(-HALFSIZE, HALFSIZE, HALFSIZE);
vertices[3][3] = object->newVertex( HALFSIZE, HALFSIZE, HALFSIZE);
// face -z
vertices[4][0] = object->newVertex(-HALFSIZE, -HALFSIZE, -HALFSIZE);
vertices[4][1] = object->newVertex(-HALFSIZE, HALFSIZE, -HALFSIZE);
vertices[4][2] = object->newVertex( HALFSIZE, HALFSIZE, -HALFSIZE);
vertices[4][3] = object->newVertex( HALFSIZE, -HALFSIZE, -HALFSIZE);
// face +z
vertices[5][0] = object->newVertex( HALFSIZE, -HALFSIZE, HALFSIZE);
vertices[5][1] = object->newVertex( HALFSIZE, HALFSIZE, HALFSIZE);
vertices[5][2] = object->newVertex(-HALFSIZE, HALFSIZE, HALFSIZE);
vertices[5][3] = object->newVertex(-HALFSIZE, -HALFSIZE, HALFSIZE);
// create triangles
for (int i=0; i<6; i++)
{
object->newTriangle(vertices[i][0], vertices[i][1], vertices[i][2]);
object->newTriangle(vertices[i][0], vertices[i][2], vertices[i][3]);
}
// create a texture
texture = new cTexture2D();
object->setTexture(texture);
object->setUseTexture(true);
// set material properties to light gray
object->m_material.m_ambient.set(0.5f, 0.5f, 0.5f, 1.0f);
object->m_material.m_diffuse.set(0.7f, 0.7f, 0.7f, 1.0f);
object->m_material.m_specular.set(1.0f, 1.0f, 1.0f, 1.0f);
object->m_material.m_emission.set(0.0f, 0.0f, 0.0f, 1.0f);
// compute normals
object->computeAllNormals();
// display triangle normals
object->setShowNormals(true);
// set length and color of normals
object->setNormalsProperties(0.1, cColorf(1.0, 0.0, 0.0), true);
// compute a boundary box
object->computeBoundaryBox(true);
// get dimensions of object
double size = cSub(object->getBoundaryMax(), object->getBoundaryMin()).length();
// resize object to screen
object->scale( 2.0 * tool->getWorkspaceRadius() / size);
// compute collision detection algorithm
object->createAABBCollisionDetector(1.01 * proxyRadius, true, false);
// define a default stiffness for the object
object->setStiffness(stiffnessMax, true);
// define friction properties
object->setFriction(0.2, 0.5, true);
//-----------------------------------------------------------------------
// OPEN GL - WINDOW DISPLAY
//-----------------------------------------------------------------------
// initialize GLUT
glutInit(&argc, argv);
// retrieve the resolution of the computer display and estimate the position
// of the GLUT window so that it is located at the center of the screen
int screenW = glutGet(GLUT_SCREEN_WIDTH);
int screenH = glutGet(GLUT_SCREEN_HEIGHT);
int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
int windowPosY = (screenH - WINDOW_SIZE_H) / 2;
// initialize the OpenGL GLUT window
glutInitWindowPosition(windowPosX, windowPosY);
glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(argv[0]);
glutDisplayFunc(updateGraphics);
glutKeyboardFunc(keySelect);
glutReshapeFunc(resizeWindow);
glutSetWindowTitle("CHAI 3D");
// create a mouse menu (right button)
glutCreateMenu(menuSelect);
glutAddMenuEntry("full screen", OPTION_FULLSCREEN);
glutAddMenuEntry("window display", OPTION_WINDOWDISPLAY);
glutAttachMenu(GLUT_RIGHT_BUTTON);
//-----------------------------------------------------------------------
// START SIMULATION
//-----------------------------------------------------------------------
// simulation in now running
simulationRunning = true;
// create a thread which starts the main haptics rendering loop
cThread* hapticsThread = new cThread();
hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);
// start the main graphics rendering loop
glutMainLoop();
// close everything
close();
// exit
return (0);
}
void TForm1::compute_spring_forces()
{
double curtime = g_timer.GetTime();
if (g_last_iteration_time < 0)
{
g_last_iteration_time = curtime;
return;
}
double elapsed = curtime - g_last_iteration_time;
g_last_iteration_time = curtime;
unsigned int i;
// Clear the force that's applied to each ball
for(i=0; i<m_active_balls.size(); i++)
{
m_active_balls[i]->current_force.set(0,0,0);
}
if (simulationOn)
{
CBall* b = m_active_balls[0];
cVector3d old_p = b->getPos();
b->setPos(tool->m_deviceGlobalPos);
b->m_velocity = cDiv(elapsed,cSub(b->getPos(),old_p));
}
// Compute the current length of each spring and apply forces
// on each mass accordingly
for(i=0; i<m_active_springs.size(); i++)
{
CSpring* s = m_active_springs[i];
double d = cDistance(s->m_endpoint_1->getPos(),s->m_endpoint_2->getPos());
s->m_current_length = d;
// This spring's deviation from its rest length
//
// (positive deviation -> spring is too long)
double x = s->m_current_length - s->m_rest_length;
// Apply a force to ball 1 that pulls it toward ball 2
// when the spring is too long
cVector3d f1 = cMul(s->m_spring_constant*x*1.0,
cSub(s->m_endpoint_2->getPos(),s->m_endpoint_1->getPos()));
s->m_endpoint_1->current_force.add(f1);
// Add the opposite force to ball 2
s->m_endpoint_2->current_force.add(cMul(-1.0,f1));
}
// Update velocities and positions based on forces
for(i=0; i<m_active_balls.size(); i++)
{
CBall* b = m_active_balls[i];
// Certain forces don't get applied to the "haptic ball"
// when haptics are enabled...
if (simulationOn == 0 || i != 0) {
cVector3d f_damping = cMul(DAMPING_CONSTANT,b->m_velocity);
b->current_force.add(f_damping);
}
cVector3d f_gravity(0,GRAVITY_CONSTANT*b->m_mass,0);
b->current_force.add(f_gravity);
cVector3d p = b->getPos();
if (p.y - b->m_radius < FLOOR_Y_POSITION) {
double penetration = FLOOR_Y_POSITION - (p.y - b->m_radius);
b->current_force.add(0,m_floor_spring_constant*penetration,0);
}
cVector3d f_floor(0,0,0);
cVector3d a = cDiv(b->m_mass,b->current_force);
b->m_velocity.add(cMul(elapsed,a));
// We handle the 0th ball specially when haptics is enabled
if (simulationOn == 0 || i != 0) {
p.add(cMul(elapsed,b->m_velocity));
b->setPos(p);
}
}
// Set the haptic force appropriately to reflect the force
// applied to ball 0
m_haptic_force = cMul(HAPTIC_FORCE_CONSTANT,m_active_balls[0]->current_force);
}
BOOL Cmesh_mesh_collisionsApp::InitInstance() {
AfxEnableControlContainer();
#ifdef _AFXDLL
Enable3dControls(); // Call this when using MFC in a shared DLL
#else
Enable3dControlsStatic(); // Call this when linking to MFC statically
#endif
g_main_dlg = new Cmesh_mesh_collisionsDlg;
m_pMainWnd = g_main_dlg;
g_main_dlg->Create(IDD_mesh_mesh_collisions_DIALOG,NULL);
// Now we should have a display context to work with...
// Create a world and set a white background color
world = new cWorld();
world->setBackgroundColor(1.0,1.0,1.0);
// Create a camera and set its position, look-at point, and orientation (up-direction)
camera = new cCamera(world);
int result = camera->set(cVector3d(0,0,4), cVector3d(0,0,0), cVector3d(0,1,0));
// Create, enable, and position a light source
light = new cLight(world);
light->setEnabled(true);
light->setPos(cVector3d(0,1,4));
// Create a display for graphic rendering
viewport = new cViewport(g_main_dlg->m_gl_area_hwnd, camera, false);
// Load a gear mesh from a .3DS file
object = new cMesh(world);
object->loadFromFile("resources\\models\\small_gear.3ds");
// Scale the object to fit nicely in our viewport
// compute size of object
object->computeBoundaryBox(true);
cVector3d min = object->getBoundaryMin();
cVector3d max = object->getBoundaryMax();
// This is the "size" of the object
cVector3d span = cSub(max, min);
double size = cMax(span.x, cMax(span.y, span.z));
// We'll center all vertices, then multiply by this amount,
// to scale to the desired size.
double scaleFactor = 2.0 / size;
object->scale(scaleFactor);
// Tell him to compute a bounding box...
object->computeBoundaryBox(true);
// Build a nice collision-detector for this object
object->createSphereTreeCollisionDetector(true,true);
// Automatically compute normals for all triangles
object->computeAllNormals();
// Translate and rotate so that the airplane is flying towards the right of the screen
object->translate(0.7, 0.0, 0.0);
object->rotate(cVector3d(0,1,0),-90.0 * 3.14159 / 180.0);
object->rotate(cVector3d(1,0,0),-30.0 * 3.14159 / 180.0);
object->computeGlobalPositions(false);
// Use vertex colors so we can see which triangles collide
object->useColors(true, true);
// Add the mesh object to the world
world->addChild(object);
world->computeGlobalPositions();
m_show_all = 1;
return TRUE;
}
Carro::Carro(dynamicWorld* world):dynamicObject(world,0.5*LARGURACARRO*COMPRIMENTOCARRO,true) {
steeringAngle = 0;
//Instancia o chassi e o meio
meio = new cGenericObject();
chassi = new cGenericObject();
meio->addChild(chassi);
//Adiciona o meio ao carro propriamente dito
this->addChild(meio);
//Inicializa a roda1
roda1 = new cMesh(world);
roda1->loadFromFile("roda_simples.obj");
roda1->computeBoundaryBox(true);
cVector3d min = roda1->getBoundaryMin();
cVector3d max = roda1->getBoundaryMax();
cVector3d meio = cMul(-0.5,cAdd(min,max));
cVector3d span = cSub(max, min);
for(int i=0;i<roda1->getNumVertices(true);i++)
roda1->getVertex(i,true)->translate(meio);
double size = cMax(span.x, cMax(span.y, span.z));
double scaleFactor = 2*RAIORODA / size;
roda1->scale(scaleFactor);
chassi->addChild(roda1);
roda1->translate(0,RAIORODA,LARGURACARRO/2.0);
roda1->rotate(cVector3d(1,0,0),3.1415/2.0);
//Instancia e inicializa a roda2
roda2 = new cMesh(world);
roda2->loadFromFile("roda_simples.obj");
for(int i=0;i<roda2->getNumVertices(true);i++)
roda2->getVertex(i,true)->translate(meio);
roda2->scale(scaleFactor);
chassi->addChild(roda2);
roda2->translate(0,RAIORODA,-LARGURACARRO/2.0);
roda2->rotate(cVector3d(1,0,0),-3.1415/2.0);
//Instanci os eixos do carro
eixo1 = new cGenericObject();
chassi->addChild(eixo1);
eixo2 = new cGenericObject();
chassi->addChild(eixo2);
//Instancia e inicializa a roda3
roda3 = new cMesh(world);
roda3->loadFromFile("roda_simples.obj");
for(int i=0;i<roda3->getNumVertices(true);i++)
roda3->getVertex(i,true)->translate(meio);
roda3->scale(scaleFactor);
eixo1->addChild(roda3);
eixo1->translate(DISTANCIAEIXOS,RAIORODA,LARGURACARRO/2.0);
roda3->rotate(cVector3d(1,0,0),3.1415/2.0);
//Instancia e inicializa a roda4
roda4 = new cMesh(world);
roda4->loadFromFile("roda_simples.obj");
for(int i=0;i<roda4->getNumVertices(true);i++)
roda4->getVertex(i,true)->translate(meio);
roda4->scale(scaleFactor);
eixo2->addChild(roda4);
eixo2->translate(DISTANCIAEIXOS,RAIORODA,-LARGURACARRO/2.0);
roda4->rotate(cVector3d(1,0,0),-3.1415/2.0);
//Instancia e inicializa a carroceria
carroceria = new cMesh(world);
carroceria->loadFromFile("ferrari.3ds");
chassi->addChild(carroceria);
carroceria->computeBoundaryBox(true);
min = carroceria->getBoundaryMin();
max = carroceria->getBoundaryMax();
span = cSub(max, min);
meio = cMul(-0.5,cAdd(min,max));
for(int i=0;i<carroceria->getNumVertices(true);i++)
carroceria->getVertex(i,true)->translate(meio);
size = cMax(span.x, cMax(span.y, span.z));
scaleFactor = COMPRIMENTOCARRO / size;
carroceria->scale(scaleFactor);
carroceria->rotate(cVector3d(0,1,0),3.1415/2.0);
carroceria->rotate(cVector3d(0,0,1),3.1415/2.0);
//recalcula dimensões
carroceria->computeBoundaryBox(true);
min = carroceria->getBoundaryMin();
max = carroceria->getBoundaryMax();
span = cSub(max, min);
double altura= cMin(span.x, cMin(span.y, span.z));
// posiciona carroceria: assumindo que altura do chão é 0.3* raio da roda
// levanta meia altura para chao ficar no plano x,z
carroceria->translate(DISTANCIAEIXOS/1.85,altura*0.5+RAIORODA*0.3,0);
carroceria->useColors(true, true);
carroceria->useMaterial(false,true);
}
void updateHaptics(void)
{
// reset clock
simClock.reset();
// main haptic simulation loop
while(simulationRunning)
{
// compute global reference frames for each object
world->computeGlobalPositions(true);
// update position and orientation of tool
tool->updatePose();
// compute interaction forces
tool->computeInteractionForces();
// send forces to device
tool->applyForces();
// stop the simulation clock
simClock.stop();
// read the time increment in seconds
double timeInterval = simClock.getCurrentTimeSeconds();
// restart the simulation clock
simClock.reset();
simClock.start();
// temp variable to compute rotational acceleration
cVector3d rotAcc(0,0,0);
// check if tool is touching an object
cGenericObject* objectContact = tool->m_proxyPointForceModel->m_contactPoint0->m_object;
if (objectContact != NULL)
{
// retrieve the root of the object mesh
cGenericObject* obj = objectContact->getSuperParent();
// get position of cursor in global coordinates
cVector3d toolPos = tool->m_deviceGlobalPos;
// get position of object in global coordinates
cVector3d objectPos = obj->getGlobalPos();
// compute a vector from the center of mass of the object (point of rotation) to the tool
cVector3d vObjectCMToTool = cSub(toolPos, objectPos);
// compute acceleration based on the interaction forces
// between the tool and the object
if (vObjectCMToTool.length() > 0.0)
{
// get the last force applied to the cursor in global coordinates
// we negate the result to obtain the opposite force that is applied on the
// object
cVector3d toolForce = cNegate(tool->m_lastComputedGlobalForce);
// compute effective force to take into account the fact the object
// can only rotate around a its center mass and not translate
cVector3d effectiveForce = toolForce - cProject(toolForce, vObjectCMToTool);
// compute the resulting torque
cVector3d torque = cMul(vObjectCMToTool.length(), cCross( cNormalize(vObjectCMToTool), effectiveForce));
// update rotational acceleration
const double OBJECT_INERTIA = 0.4;
rotAcc = (1.0 / OBJECT_INERTIA) * torque;
}
}
// update rotational velocity
rotVel.add(timeInterval * rotAcc);
// set a threshold on the rotational velocity term
const double ROT_VEL_MAX = 10.0;
double velMag = rotVel.length();
if (velMag > ROT_VEL_MAX)
{
rotVel.mul(ROT_VEL_MAX / velMag);
}
// add some damping too
const double DAMPING_GAIN = 0.1;
rotVel.mul(1.0 - DAMPING_GAIN * timeInterval);
// if user switch is pressed, set velocity to zero
if (tool->getUserSwitch(0) == 1)
{
rotVel.zero();
}
// compute the next rotation configuration of the object
if (rotVel.length() > CHAI_SMALL)
{
object->rotate(cNormalize(rotVel), timeInterval * rotVel.length());
}
}
// exit haptics thread
simulationFinished = true;
}
