void SolveStep::verlet(double delta_t, int objectNumber, Object &mainbody, System &newSystem, double addtime, std::string dimension) { int i = objectNumber; System tempSystem = newSystem; Object &tempBody = tempSystem.objectlist[i]; tempBody.setPosition(mainbody.getPosition()+mainbody.getVelocity()*delta_t + mainbody.getAcceleration()*delta_t*delta_t*0.5); //pos_i+1 tempSystem.acceleration(tempBody, i, addtime,dimension); tempBody.setVelocity(mainbody.getVelocity() + 0.5* (mainbody.getAcceleration()+ tempBody.getAcceleration())*delta_t); //vel_i+1 mainbody.setPosition(tempBody.getPosition()); mainbody.setVelocity(tempBody.getVelocity()); mysolarsystem_.acceleration(mainbody, i, addtime,dimension); }
void SpaceObject::matchVelocity(std::string objectName) { Object *o = ObjectMgr::get().find(objectName); if (o && objectName != id) { Vector deltaV = o->getVelocity() - velocity; if (deltaV.magnitude() > 0.1f) { Vector thrustDir = deltaV.normalize(); Vector axis = (look % thrustDir).normalize(); if (axis.magnitude() < 0.01f) axis = up; float ang = look.angle(thrustDir); if (ang < 0.01f || fabs(ang - 180.0f) < 0.1f) axis = up; if (ang > 0.0001f) { if (ang > topTurnSpeed * GlobalTime::get().frameSec) ang = topTurnSpeed * GlobalTime::get().frameSec; rotateAxis(axis, ang); } thrust(100.0f); } } }
double System::kineticEnergi() const { double energy = 0; for (int i = 0 ; i < numberOfObject ; ++i) { const Object movingObject = objectlist[i]; energy += 0.5 * movingObject.getMass() * arma::dot(movingObject.getVelocity(), movingObject.getVelocity()) ; } return energy; }
//NOTE - NEED TO FIX LIKE WITH STOP AND MATCHVELOCITY //180* BUG void SpaceObject::track(std::string objectName) { Object *o = ObjectMgr::get().find(objectName); if (o && objectName != id) { //Vector from your position to object being tracked Vector delta = o->getPos() - pos; float seconds = ((-1 * (velocity - o->getVelocity()).magnitude() + sqrt( (velocity - o->getVelocity()).magnitude() * (velocity - o->getVelocity()).magnitude() + 2 * topAccel * delta.magnitude())) / (topAccel)); float seconds2 = ((-1 * (velocity - o->getVelocity()).magnitude() - sqrt( (velocity - o->getVelocity()).magnitude() * (velocity - o->getVelocity()).magnitude() + 2 * topAccel * delta.magnitude())) / (topAccel)); if (seconds2 > seconds) seconds = seconds2; //float seconds = sqrt((delta.magnitude()*2.0f)/topAccel); //Distance 1s from now Vector shootFor = delta + (o->getVelocity() - velocity) * seconds; shootFor = shootFor.normalize(); Vector axis = (look % shootFor).normalize(); //std::cerr<<"youch!!!!\n"; if (axis.magnitude() < 0.01f) axis = up; float ang = look.angle(shootFor); if (ang < 0.01f || fabs(ang - 180.0f) < 0.1f) axis = up; if (ang > 0.0001f) { if (ang > topTurnSpeed * GlobalTime::get().frameSec) ang = topTurnSpeed * GlobalTime::get().frameSec; rotateAxis(axis, ang); } thrust(100.0f); //if(detectCollision(o)) done=true; } }
void System::acceleration(Object &mainObject, int i, double time, std::string dimension) { arma::Col<double> tempAcceleration(3); arma::Col<double> acceleration(3); acceleration.zeros(); for (int j = 0; j < numberOfObject; ++j) { if (j!=i) { const Object tempObject = objectlist[j]; Distance d; arma::Col<double> position1, position2; position1 = mainObject.getPosition(); position2 = tempObject.getPosition()+ time*tempObject.getVelocity(); double R = d.twoObjects(position1, position2); if (dimension == "ly") { tempAcceleration = -(tempObject.getMass()* (mainObject.getPosition()-tempObject.getPosition())/ (R*(R*R+epsilon_*epsilon_)));// ly/yr] } else if(dimension == "AU") { tempAcceleration = -4*PI*PI*tempObject.getMass()* (mainObject.getPosition()-tempObject.getPosition())/ (R*(R*R+epsilon_*epsilon_));// ly/yr] } else { std::cout << "dimension must be AU or ly"<< std::endl; } acceleration += tempAcceleration; } //end if } //end for mainObject.setAcceleration(acceleration); }
double System::boundKineticEnergi(double limit) const { double energy = 0; arma::Col<double> center(3), position(3); center.zeros(); Distance d; for (int i = 0 ; i < numberOfObject ; ++i) { Object movingObject = objectlist[i]; position = movingObject.getPosition(); if (d.twoObjects(center, position)<limit) { energy += 0.5 * movingObject.getMass() * arma::dot(movingObject.getVelocity(), movingObject.getVelocity()) ; } } return energy; }
double System::maxTimestep(int i) { Distance d; const Object mainObject = objectlist[i]; int k; if (i == 0) { k=1; } else { k=0; } Object tempObject = objectlist[k]; arma::Col<double> position1(3), position2(3); arma::Col<double> mainVel = mainObject.getVelocity(); position1 = mainObject.getPosition(); position2 = tempObject.getPosition(); double R = d.twoObjects(position1, position2); arma::Col<double> tempVel = tempObject.getVelocity()-mainVel; double V = std::sqrt(arma::dot(tempVel,tempVel)); double maxTime; if (V>1e-16) { maxTime = R/V; } else { maxTime = R/1e-16; } double temptime; for (int j = 1; j < numberOfObject; ++j) { if (j!=i) { tempObject = objectlist[j]; position2 = tempObject.getPosition(); R = d.twoObjects(position1, position2); tempVel = tempObject.getVelocity()-mainVel; V = std::sqrt(arma::dot(tempVel,tempVel)); if (V>1e-16) { maxTime = R/V; } else { maxTime = R/1e-16; } temptime = R/V; if (temptime<maxTime) { maxTime = temptime; } } //end if } //end for return maxTime; }
void SolveStep::rk4Step(double delta_t, int objectNumber, Object &mainbody, System &newSystem, double addtime, std::string dimension) { int i = objectNumber; System tempSystem = newSystem; Object &tempBody = tempSystem.objectlist[i]; arma::Col<double> k1Pos(3), k1Vel(3), k2Pos(3), k2Vel(3), k3Pos(3), k3Vel(3), k4Pos(3), k4Vel(3); // Calculate k1 arma::Col<double> dPosdt, dVeldt; dPosdt = tempBody.getVelocity(); dVeldt = tempBody.getAcceleration(); k1Pos = dPosdt*delta_t; k1Vel = dVeldt*delta_t; tempBody.setPosition(mainbody.getPosition() + k1Pos*0.5); tempBody.setVelocity(mainbody.getVelocity() + k1Vel*0.5); // k2 tempSystem.acceleration(tempBody, i, addtime,dimension); dPosdt = tempBody.getVelocity(); dVeldt = tempBody.getAcceleration(); k2Pos = dPosdt*delta_t; k2Vel = dVeldt*delta_t; tempBody.setPosition(mainbody.getPosition() + k2Pos*0.5); tempBody.setVelocity( mainbody.getVelocity() + k2Vel*0.5); // k3 tempSystem.acceleration(tempBody, i, addtime,dimension); dPosdt = tempBody.getVelocity(); dVeldt = tempBody.getAcceleration(); k3Pos = dPosdt*delta_t; k3Vel = dVeldt*delta_t; tempBody.setPosition( mainbody.getPosition() + k3Pos); tempBody.setVelocity( mainbody.getVelocity() + k3Vel); // k4 tempSystem.acceleration(tempBody, i, addtime,dimension); dPosdt = tempBody.getVelocity(); dVeldt = tempBody.getAcceleration(); k4Pos = dPosdt*delta_t; k4Vel = dVeldt*delta_t; tempBody.setPosition( mainbody.getPosition() + 1.0/6.0 * (k1Pos + 2.*k2Pos + 2.*k3Pos + k4Pos)); tempBody.setVelocity( mainbody.getVelocity() + 1.0/6.0 * (k1Vel + 2.*k2Vel + 2.*k3Vel + k4Vel)); mainbody.setPosition(tempBody.getPosition()); mainbody.setVelocity(tempBody.getVelocity()); newSystem.acceleration(mainbody, i, addtime,dimension); }
bool handleCollision(Contact *contact) { Object *source = contact->object1; Object *target = contact->object2; float *normal = contact->normal; float *contactpoint = contact->position; float sourcevelocity[3], targetvelocity[3]; float sourcecontactpoint[3], targetcontactpoint[3]; vectorSub(sourcecontactpoint, contactpoint, source->position); source->getVelocity(sourcevelocity, sourcecontactpoint); if (target == NULL) { vectorSet(targetcontactpoint, 0, 0, 0); vectorSet(targetvelocity, 0, 0, 0); } else { vectorSub(targetcontactpoint, contactpoint, target->position); target->getVelocity(targetvelocity, targetcontactpoint); } float deltavelocity[3]; vectorSub(deltavelocity, sourcevelocity, targetvelocity); float dot = vectorDot(deltavelocity, normal); // if (fabs(dot) < EPSILON) return false; // if (dot > -1.0e-5 && dot < 1.0e-5) return false; // if (dot >= 0) return false; if (dot > -1.0e-5) return false; float invmass1; invmass1 = source->invmass; float invmass2; if (target == NULL) invmass2 = 0; else invmass2 = target->invmass; float t1; if (source->invmomentofinertia == 0) { t1 = 0; } else { float v1[3]; vectorCross(v1, sourcecontactpoint, normal); vectorScale(v1, source->invmomentofinertia); float w1[3]; vectorCross(w1, v1, sourcecontactpoint); t1 = vectorDot(normal, w1); } float t2; if (target == NULL || target->invmomentofinertia == 0) { t2 = 0; } else { float v1[3]; vectorCross(v1, targetcontactpoint, normal); vectorScale(v1, target->invmomentofinertia); float w1[3]; vectorCross(w1, v1, targetcontactpoint); t2 = vectorDot(normal, w1); } float denominator = invmass1 + invmass2 + t1 + t2; float e = 1.0 - COLLISIONFRICTION; float impulsesize = (1 + e) * dot / denominator; // printf("%f\n", impulsesize); float impulse[3]; vectorScale(impulse, normal, impulsesize); float friction[3]; vectorScale(friction, normal, vectorDot(deltavelocity, normal)); vectorAdd(friction, deltavelocity); vectorNormalize(friction); float frictionsize = 10 * KINETICFRICTION * dot / denominator; float maxfrictionsize = 0.1 * vectorLength(deltavelocity); if (frictionsize < -maxfrictionsize) frictionsize = -maxfrictionsize; vectorScale(friction, -frictionsize); vectorAdd(impulse, friction); if (target != NULL) { target->addImpulse(impulse, targetcontactpoint); target->calculateStateVariables(); } float speed; float speed2[3]; if (target != NULL && source != NULL) { // float kvel[3]; // source->getVelocity(kvel); float k = vectorLength(sourcevelocity) * 0.1; // if (k > 1) k = 1; speed = -impulsesize * target->invmass * k; vectorScale(speed2, impulse, target->invmass * k); /*float kvel[3]; source->getVelocity(kvel); float k = 0;//vectorDot(speed2, kvel); if (k < EPSILON) k = 0; speed *= k; vectorScale(speed2, k); if (k > 0) */ target->hitForce(speed, speed2, source); } vectorScale(impulse, -1); source->addImpulse(impulse, sourcecontactpoint); source->calculateStateVariables(); // vectorScale(speed, source->invmass); if (target != NULL && source != NULL) { // float kvel[3]; // target->getVelocity(kvel); float k = vectorLength(targetvelocity) * 0.1; // if (k > 1) k = 1; speed = -impulsesize * source->invmass * k; vectorScale(speed2, impulse, source->invmass * k); /*float kvel[3]; target->getVelocity(kvel); float k = 0;//vectorDot(speed2, kvel); if (k < EPSILON) k = 0; speed *= k; vectorScale(speed2, k); if (k > 0) */ source->hitForce(speed, speed2, target); } return true; }
bool handleLink(ObjectLink *link) { if (!link->enabled) return false; Object *source = link->object1; Object *target = link->object2; float normal[3]; float contactpoint1[3], contactpoint2[3]; source->transformPoint(contactpoint1, link->point1); target->transformPoint(contactpoint2, link->point2); float diff[3]; vectorSub(diff, contactpoint1, contactpoint2); vectorNormalize(normal, diff); float strength = vectorDot(diff, diff); if (strength < 1.0e-5) return false; float sourcevelocity[3], targetvelocity[3]; float sourcecontactpoint[3], targetcontactpoint[3]; vectorSub(sourcecontactpoint, contactpoint1, source->position); source->getVelocity(sourcevelocity, sourcecontactpoint); vectorSub(targetcontactpoint, contactpoint2, target->position); target->getVelocity(targetvelocity, targetcontactpoint); float deltavelocity[3]; vectorSub(deltavelocity, sourcevelocity, targetvelocity); float dot = vectorDot(deltavelocity, normal); // if (fabs(dot) < EPSILON) return false; // if (dot > -1.0e-5 && dot < 1.0e-5) return false; // if (dot >= 0) return false; // if (dot > -1.0e-5) return false; float invmass1 = source->invmass; float invmass2 = target->invmass; float t1; if (source->invmomentofinertia == 0) { t1 = 0; } else { float v1[3]; vectorCross(v1, sourcecontactpoint, normal); vectorScale(v1, source->invmomentofinertia); float w1[3]; vectorCross(w1, v1, sourcecontactpoint); t1 = vectorDot(normal, w1); } float t2; if (target->invmomentofinertia == 0) { t2 = 0; } else { float v1[3]; vectorCross(v1, targetcontactpoint, normal); vectorScale(v1, target->invmomentofinertia); float w1[3]; vectorCross(w1, v1, targetcontactpoint); t2 = vectorDot(normal, w1); } float denominator = invmass1 + invmass2 + t1 + t2; float impulsesize = (dot + strength * 100) / denominator; // printf("%f\n", impulsesize); float impulse[3]; vectorScale(impulse, normal, impulsesize); target->addImpulse(impulse, targetcontactpoint); target->calculateStateVariables(); vectorScale(impulse, -1); source->addImpulse(impulse, sourcecontactpoint); source->calculateStateVariables(); return true; }