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; }