static void addForce(struct star* star_A, struct star* star_B) { prec dx = readXPos(star_A) - readXPos(star_B); prec dy = readYPos(star_A) - readYPos(star_B); prec distance = (double)sqrt(pow(dx, 2.0) + pow(dy, 2.0)); prec force = (prec)(G * (double)readMass(star_A) * (double)readMass(star_B) / distance); prec fx = force * dx / distance; prec fy = force * dy / distance; prec newXForceA = readXForce(star_A) - fx; prec newXForceB = readXForce(star_B) + fx; prec newYForceA = readYForce(star_A) - fy; prec newYForceB = readYForce(star_B) + fy; newForce(star_A, newXForceA, newYForceA); newForce(star_B, newXForceB, newYForceB); }
void updateHaptics(void) { // main haptic simulation loop while(simulationRunning) { // for each device int i=0; while (i < numHapticDevices) { // read position of haptic device cVector3d newPosition; hapticDevices[i]->getPosition(newPosition); // read orientation of haptic device cMatrix3d newRotation; hapticDevices[i]->getRotation(newRotation); // update position and orientation of cursor cursors[i]->setPos(newPosition); cursors[i]->setRot(newRotation); // read linear velocity from device cVector3d linearVelocity; hapticDevices[i]->getLinearVelocity(linearVelocity); // update arrow velocityVectors[i]->m_pointA = newPosition; velocityVectors[i]->m_pointB = cAdd(newPosition, linearVelocity); // read user button status bool buttonStatus; hapticDevices[i]->getUserSwitch(0, buttonStatus); // adjustthe color of the cursor according to the status of // the user switch (ON = TRUE / OFF = FALSE) if (buttonStatus) { cursors[i]->m_material = matCursorButtonON; } else { cursors[i]->m_material = matCursorButtonOFF; } // compute a reaction force cVector3d newForce (0,0,0); // apply force field if (useForceField) { double Kp = 20.0; // [N/m] cVector3d force = cMul(-Kp, newPosition); newForce.add(force); } // apply viscosity if (useDamping) { cHapticDeviceInfo info = hapticDevices[i]->getSpecifications(); double Kv = info.m_maxLinearDamping; cVector3d force = cMul(-Kv, linearVelocity); newForce.add(force); } // send computed force to haptic device hapticDevices[i]->setForce(newForce); // increment counter i++; } } // exit haptics thread simulationFinished = true; }
void updateHaptics(void) { double timeV = 0.0; cLabel* label = new cLabel(); cLabel* label2 = new cLabel(); rootLabels->addChild(label); rootLabels->addChild(label2); label->setPos(0, 0, 0); label2->setPos(0, -20, 0); label->m_fontColor.set(1.0, 1.0, 1.0); label2->m_fontColor.set(1.0, 1.0, 1.0); // main haptic simulation loop while(simulationRunning) { float deltaTime = pClock.getCurrentTimeSeconds() - lastTime; lastTime = pClock.getCurrentTimeSeconds(); cVector3d equilibrium (0.0, 0.0, 0.0); cVector3d pos0 (0.0, 0.0, 0.0), pos1 (0.0, 0.0, 0.0); // for each device int i=0; while (i < numHapticDevices) { // read position of haptic device cVector3d newPosition; hapticDevices[i]->getPosition(newPosition); newPosition = deviceToWorld(newPosition, i); ((i == 0)? pos0 : pos1) = newPosition; // read orientation of haptic device cMatrix3d newRotation; hapticDevices[i]->getRotation(newRotation); // update position and orientation of cursor cursors[i]->setPos(newPosition); cursors[i]->setRot(newRotation); // read linear velocity from device cVector3d linearVelocity; hapticDevices[i]->getLinearVelocity(linearVelocity); // update arrow // velocityVectors[i]->m_pointA = newPosition; // velocityVectors[i]->m_pointB = cAdd(newPosition, linearVelocity); // read user button status bool buttonStatus; hapticDevices[i]->getUserSwitch(0, buttonStatus); // adjustthe color of the cursor according to the status of // the user switch (ON = TRUE / OFF = FALSE) if (i == 0) cursors[i]->m_material = matCursor2; else cursors[i]->m_material = matCursor1; // increment counter i++; } double f0, f1; f0 = pos0.length(); f1 = pos1.length(); f0 = f0/(f0 + f1); f1 = 1.0 - f0; equilibrium = pos1 + (pos0 - pos1)*f0; // Update the position of the sun cVector3d dir = pos1 - pos0; double dist = dir.length(); dir.normalize(); double vibrationSpeed = 20.0; double vibrationAmount = 0.02; //sun->setPos(sun->getPos()*(1.0 - deltaTime*speed) + equilibrium*(deltaTime*speed)); //timeV += deltaTime*vibrationSpeed*(0.7 - cAbs(f0 - 0.5)*2.0); sun->setPos(equilibrium /*+ vibrationAmount*dir*cSinRad(timeV)*/); // Update logic if (!calibrationFinished) { label->m_string = "Calibrating, please move the haptic devices around in order to determine their limitations in movement."; label2->m_string = "Press 'c' to finish calibration."; if (sun->getPos().x < min.x) min.x = sun->getPos().x; if (sun->getPos().y < min.y) min.y = sun->getPos().y; if (sun->getPos().z < min.z) min.z = sun->getPos().z; if (sun->getPos().x > max.x) max.x = sun->getPos().x; if (sun->getPos().y > max.y) max.y = sun->getPos().y; if (sun->getPos().z > max.z) max.z = sun->getPos().z; } else if (logic->isReady()) { if (logic->gameIsOver() && !scoreDisplayed) { std::stringstream strs; strs << logic->playTime(); std::string playString = strs.str(); // define its position, color and string message label->m_string = "Congratulation! Your Time: " + playString; label2->m_string = "Press 'r' to restart!"; for (i = 0; i < numHapticDevices; i++) { cVector3d zero(0.0, 0.0, 0.0); hapticDevices[i]->setForce(zero); } scoreDisplayed = true; } else if (!scoreDisplayed) { label->m_string = ""; label2->m_string = ""; logic->update(deltaTime); } for (i = 0; i < numHapticDevices; i++) { // compute a reaction force cVector3d newForce (0,0,0); cVector3d devicePosition; hapticDevices[i]->getPosition(devicePosition); devicePosition = deviceToWorld(devicePosition, i); double k = 0.4; if (test == 1) k = 0.3; double dist = (devicePosition - sun->getPos()).length(); //dist=dist-0.1; newForce = k*(devicePosition - sun->getPos())/(dist*dist*dist); //double intensity = (newForce.length())*1.0; //newForce.normalize(); //newForce *= intensity; // newForce = k*(devicePosition - sun->getPos())/(dist*dist*dist); if (i == 0) { // Device on positive X (RIGHT) newForce.x *= -1.0; newForce.y *= -1.0; } // send computed force to haptic device // bool status = true; // if (hapticDevices[i]->getUserSwitch(0)) // printf("button pressed\n"); // Check if the sphere is in the target area. If so, vibrate cVector3d vibrationForce(0.0, 0.0, 0.0); if (logic->sphereInTarget() && !logic->gameIsOver()) { Cube* target = logic->getTarget(); double dist = target->getPos().distance(sun->getPos()); double factor = 1.0 - dist/(target->size/2.0); timeV += deltaTime * (0.5 + factor/2.0); double f = 2*cSinRad(40*timeV); vibrationForce = cVector3d(f, f, f); } newForce += vibrationForce; if (test <= 2 || i == 0) hapticDevices[i]->setForce(newForce); else { cVector3d zero; zero.zero(); hapticDevices[i]->setForce(zero); } } } } // exit haptics thread simulationFinished = true; }
static void resetForce(struct star* str) { newForce(str, 0, 0); }