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