vector<float> FastShift::allDirections(vector<float> shiftPoint, vector<float> shiftValue)
{
vector<int> grid_pos(n, 0);
whichGrid(shiftPoint, grid_pos);
int pos = findposition(grid_pos);
vector<float> external_force(n, 0);
vector<float> force1(n, 0);
for(int i = 0; i < selectD; i ++)
{
for(int j = -1; j <= 1; j += 2)
{
calForce(shiftPoint, pos + j * factor[selected[i]], force1);
external_force += force1;
}
}
return external_force;
}
vector<float> FastShift::maxDirectionTwoSide(vector<float> shiftPoint, vector<float> shiftValue)
{
vector<float> external_force(n, 0);
vector<float> force(n, 0);
vector<float> force1(n, 0);
bool unset = true;
float max;
int shift;
vector<int> grid_pos(n, 0);
whichGrid(shiftPoint, grid_pos);
int pos = findposition(grid_pos);
for(int i = 0; i < selectD; i ++)
{
for(int j = -1; j <= 1; j += 2)
{
calForce(shiftPoint, pos + j * factor[selected[i]], force1);
force += force1;
}
float l = l2norm(force);
if(unset)
{
max = l;
external_force = force;
shift = pos - factor[selected[i]];
unset = false;
continue;
}
if(l > max)
{
max = l;
external_force = force;
}
}
//copy(C[shift], shiftValue, n);
return external_force;
}
// Main Loop of Physics Engine
void World::update()
{
// Propogate Objects
for (uint i=0; i<bodies_.size();i++)
{
// Calculate any external forces and torque
// Simple gravitation
/*
double mu = 0.05; // ~Earth
Vec2 pos = bodies_[i]->get_pos();
double mag = pos.norm();
Vec2 external_force;
if (mag > 0.1) // have a small dead zone
external_force = -pos*(mu/(mag*mag*mag));
*/
Vec2 external_force(0,0);
double external_torque = 0;
bodies_[i]->propagate(external_force, external_torque, update_ts_);
//Vec2 Position = bodies_[i]->get_pos();
//double xpos = Position.get_x();
//double ypos = Position.get_y();
//std::cout << xpos << ' ' << ypos << std::endl;
}
// Check for Collisions
//
// Here are some notes I've taken from a paper on rigid body dynamics. (Please don't delete)
//
// There is one thing that will be required in order to perform collision detection.
// - We Need to give the objects a definite shape and size. I would like to recommend that initially we make the SCs
// Octagonal and give the octagon a radius. From there it will be easy to determine whether one spacecraft collides
// with another. And with this shape it is most likely that a corner will hit an edge (simplest collision response)
// which will simplify initial implementation.
// We also need to decide on a coefficient of restitution (epsillon) eps = 1 means the interaction was entirely elastic,
// where as eps = 0 is entirely plastic. From the coefficient of restitution we show how much energy is disipated (sound,
// damage, etc) which can later be implemented as losing health.
// - Epsillon (coefficient of restitution) (0 - 1)
// - n (normal vector of the edge that is hit pointing toward the object that comes in with the corner)
// - J (impulse)
// - I (moment of inertia)
// - r_Ap (vector perpendicular to the vector from the CG to the point of contact)
// A note about subscripts: Numbers will be used to denote parameters that different points in time (V_2 is later than
// V_1) and letters will be used to describe different objects (V_A, V_B) thus we get (V_A1, V_B1, etc)
// - V_AB1 = (V_AP1 - V_BP1)
// - n point to A due to the equation above
// - J = (-1*(1+eps)*V_AB1 dot n)/(n dot n*((1/m_A)+(1/m_B))+(((r_Ap dot n)^2)/I_A)+(((r_Bp dot n)^2)/I_B))
// - omega_A2 = omega_A1 + ((r_Ap dot J*n)/I_A) (repeat for body B)
// - V_A2 = V_A1 - ((J*n)/m_A)
// Process:
// - Calc r_Ap, r_Bp
// - Calc n
// - Calc V_AB1
// - Calc J
// - Calc omega_2 for both bodies
// - Calc V_2 for both bodies
//
// Source: chrishecker.com/Rigid_body_dynamics
// Loop
update_timer_.expires_at(update_timer_.expires_at() +
boost::posix_time::seconds(update_ts_));
update_timer_.async_wait( boost::bind(&World::update, this) );
}
