void RodForceGenerator::Update(double deltaTime) {
if(a2de::Math::IsEqual(_length, 0.0)) return;
if(_rod_ends.first == nullptr) return;
if(_rod_ends.second == nullptr) return;
auto fb = _rod_ends.first->GetComponent<a2de::PhysicsComponent>().body;
auto sb = _rod_ends.second->GetComponent<a2de::PhysicsComponent>().body;
a2de::Vector2D fbp = fb.GetPosition();
a2de::Vector2D sbp = sb.GetPosition();
double square_distance = (fbp - sbp).GetLengthSquared();
double square_length = _length * _length;
if(a2de::Math::IsEqual(square_length, square_distance)) return;
double m1 = fb.GetMass();
double m2 = sb.GetMass();
a2de::Vector2D sb_to_center = (fbp - sbp).Normalize();
a2de::Vector2D fb_to_center = (sbp - fbp).Normalize();
//Update bodies to get up-to-date F=ma calculations for projection.
fb.Update(deltaTime);
sb.Update(deltaTime);
a2de::Vector2D m1force = a2de::Vector2D::GetProjection(m1 * fb.GetAcceleration(), fb_to_center);
a2de::Vector2D m2force = a2de::Vector2D::GetProjection(m2 * sb.GetAcceleration(), sb_to_center);
double distance = std::sqrt(square_distance);
double move_distance = distance - _length;
fb.ApplyImpulse(m1force);
sb.ApplyImpulse(m2force);
double mass_sum = m1 + m2;
double m1_ratio = m1 / mass_sum;
double m2_ratio = m2 / mass_sum;
sb.SetPosition(sbp + ((sb_to_center * move_distance * m2_ratio)));
fb.SetPosition(fbp + ((fb_to_center * move_distance * m1_ratio)));
}
void CFeature::DoDamage(
const DamageArray& damages,
const float3& impulse,
CUnit* attacker,
int weaponDefID,
int projectileID
) {
// paralyzers do not damage features
if (damages.paralyzeDamageTime)
return;
if (IsInVoid())
return;
// features have no armor-type, so use default damage
float baseDamage = damages.GetDefault();
float impulseMult = float((def->drawType >= DRAWTYPE_TREE) || (udef != NULL && !udef->IsImmobileUnit()));
if (eventHandler.FeaturePreDamaged(this, attacker, baseDamage, weaponDefID, projectileID, &baseDamage, &impulseMult))
return;
// NOTE:
// for trees, impulse is used to drive their falling animation
// this also calls our SetVelocity, which puts us in the update
// queue
ApplyImpulse((impulse * moveCtrl.impulseMask * impulseMult) / mass);
// clamp in case Lua-modified damage is negative
health -= baseDamage;
health = std::min(health, def->health);
eventHandler.FeatureDamaged(this, attacker, baseDamage, weaponDefID, projectileID);
if (health <= 0.0f && def->destructable) {
FeatureLoadParams params = {featureDefHandler->GetFeatureDefByID(def->deathFeatureDefID), NULL, pos, speed, -1, team, -1, heading, buildFacing, 0};
CFeature* deathFeature = featureHandler->CreateWreckage(params, 0, false);
if (deathFeature != NULL) {
// if a partially reclaimed corpse got blasted,
// ensure its wreck is not worth the full amount
// (which might be more than the amount remaining)
deathFeature->resources.metal *= (def->metal != 0.0f) ? resources.metal / def->metal : 1.0f;
deathFeature->resources.energy *= (def->energy != 0.0f) ? resources.energy / def->energy : 1.0f;
}
featureHandler->DeleteFeature(this);
blockHeightChanges = false;
}
}
void Solver::solveVelocities(double dt)
{
int size = joint.size();
for (auto it = contacts.begin(); it != contacts.end(); it++)
it->Initialize();
for (auto it = contacts.begin(); it != contacts.end(); it++)
it->WarmStart();
for (int i = 0; i < size; i++)
joint[i]->Initialize(dt);
ms.Initialize();
for (int j = 0; j < 8; j++)
{
for (int i = 0; i < size; i++)
joint[i]->ApplyImpulse(dt);
ms.SolveVelocities();
for (auto it = contacts.begin(); it != contacts.end(); it++)
it->ApplyImpulse();
}
}
void Keyboard(unsigned char key, int x, int y)
{
switch(key)
{
case 'a': ApplyImpulse(Vec3(-1.f, 0.f, 0.f)); break;
case 'd': ApplyImpulse(Vec3(1.f, 0.f, 0.f)); break;
case 'e': ApplyImpulse(Vec3(0.f, -1.f, 0.f)); break;
case 'q': ApplyImpulse(Vec3(0.f, 1.f, 0.f)); break;
case 'w': ApplyImpulse(Vec3(0.f, 0.f, -1.f)); break;
case 's': ApplyImpulse(Vec3(0.f, 0.f, 1.f)); break;
case 27: // escape
exit(0);
break;
}
glutPostRedisplay();
}
void Game::SimulationLoop(float delta)
{
m_dt = delta;
// Update rope's representation
updateRopeVisuals();
// Mark inactive objects
putInactiveAtRest();
//-----------------------------------------------
// Integrate velocities
CalculateObjectPhysics();
// Static and dynamic collision detection
CollisionDetection(m_dt);
// Resolve collisions
for (int i=0; i<5; i++) {
DynamicCollisionResponse(m_dt);
}
// Clear manifold
m_manifold->Assess();
// Apply impulse
if (!mAppliedAirJet)
ApplyImpulse();
// Update rope physics
m_rope.simulate(delta);
// Integrate position
UpdateObjectPhysics(delta);
//-----------------------------------------------
}
void RigidBody::ApplyContact(Contact& contact)
{
Vec3 vel = dot(contact.Normal, velocity) * contact.Normal;
ApplyImpulse(-2 * vel);
}
void State::ApplyYImpulse(double y) { ApplyImpulse(0.0, y); }
void State::ApplyXImpulse(double x) { ApplyImpulse(x, 0.0); }
void State::ApplyImpulse(double x, double y) { ApplyImpulse(Vector2D(x, y)); }
void Player::MoveUp()
{
ApplyImpulse(Engine::Vector2(THRUST, THRUST));
}
void CollResponseSI::Solve()
{
//return status of our solver
int ireturnStatus;
//number of iterations
int iterations;
if(this->m_pGraph->edges_->isEmpty())
return;
int i,j;
Real deltaT = m_pWorld->timeControl_->GetDeltaT();
CPerfTimer timer0;
dTimeAssembly = 0;
dTimeSolver = 0;
dTimeSolverPost = 0;
dTimeAssemblyDry = 0;
m_dBiasFactor = 0.0;
//number of different contacts
int nContacts=0;
//in the SI framework we apply the external forces before
//we call the constraint force solver
std::vector<RigidBody*> &vRigidBodies = m_pWorld->rigidBodies_;
std::vector<RigidBody*>::iterator rIter;
for(rIter=vRigidBodies.begin();rIter!=vRigidBodies.end();rIter++)
{
RigidBody *body = *rIter;
if(!body->isAffectedByGravity())
continue;
VECTOR3 &vel = body->velocity_;
}//end for
// int count = 0;
// for(rIter=vRigidBodies.begin();rIter!=vRigidBodies.end();rIter++)
// {
//
// CRigidBody *body = *rIter;
//
// if(body->m_iShape == CRigidBody::BOUNDARYBOX || !body->IsAffectedByGravity())
// continue;
//
// VECTOR3 &pos = body->m_vCOM;
// VECTOR3 &vel = body->m_vVelocity;
// //std::cout<<"body id/remoteid/velocity: "<<body->m_iID<<" "<<body->m_iRemoteID<<" "<<body->m_vVelocity;
// body->SetAngVel(VECTOR3(0,0,0));
//
// //velocity update
// if(body->IsAffectedByGravity())
// {
// vel += m_pWorld->GetGravityEffect(body) * m_pWorld->m_pTimeControl->GetDeltaT();
// }
//
// //std::cout<<"body id/remoteid/velocity: "<<body->m_iID<<" "<<body->m_iRemoteID<<" "<<body->m_vVelocity;
//
// }//end for
timer0.Start();
m_iContactPoints = 0;
CollisionHash::iterator hiter = m_pGraph->edges_->begin();
for(;hiter!=m_pGraph->edges_->end();hiter++)
{
CollisionInfo &info = *hiter;
if(!info.m_vContacts.empty())
{
PreComputeConstants(info);
}
}
dTimeAssemblyDry+=timer0.GetTime();
//initialize the defect vector
m_vDef = VectorNr(m_iContactPoints);
timer0.Start();
//call the sequential impulses solver with a fixed
//number of iterations
for(iterations=0;iterations<10;iterations++)
{
//std::cout<<"Iteration: "<<iterations<<" ------------------------------------------------------"<<std::endl;
for(rIter=vRigidBodies.begin();rIter!=vRigidBodies.end();rIter++)
{
RigidBody *body = *rIter;
if(!body->isAffectedByGravity())
continue;
VECTOR3 &vel = body->velocity_;
//backup the velocity
body->oldVel_ = body->velocity_;
//std::cout<<"body id/remoteid/velocity: "<<body->m_iID<<" "<<body->m_iRemoteID<<" "<<body->m_vVelocity;
}//end for
hiter = m_pGraph->edges_->begin();
for(;hiter!=m_pGraph->edges_->end();hiter++)
{
CollisionInfo &info = *hiter;
if(!info.m_vContacts.empty())
{
ApplyImpulse(info);
}
}
#ifdef FC_MPI_SUPPORT
//we now have to synchronize the remote bodies
if(m_pWorld->m_myParInfo.GetID() == 0)
{
int nBodies = m_pWorld->m_pSubBoundary->m_iRemoteIDs[0].size();
//std::cout<<"Number of remotes in 0 "<<nBodies<<std::endl;
//send struct {diff,targetID}
Real *diffs = new Real[3*nBodies];
int *remotes = new int[nBodies];
Real *diffs2 = new Real[3*nBodies];
int *remotes2 = new int[nBodies];
for(int k=0;k<nBodies;k++)
{
remotes[k]=m_pWorld->m_pSubBoundary->m_iRemoteIDs[0][k];
CRigidBody *body = m_pWorld->m_vRigidBodies[m_pWorld->m_pSubBoundary->m_iRemoteBodies[0][k]];
diffs[3*k] = body->m_vVelocity.x - body->m_vOldVel.x;
diffs[3*k+1] = body->m_vVelocity.y - body->m_vOldVel.y;
diffs[3*k+2] = body->m_vVelocity.z - body->m_vOldVel.z;
/* std::cout<<"myid= 0 /id/velocity update: "<<body->m_iID<<" "<<diffs[3*k+2]<<" "<<body->m_vVelocity;
std::cout<<VECTOR3(diffs[3*k],diffs[3*k+1],diffs[3*k+2]); */
}
MPI_Send(remotes,nBodies,MPI_INT,1,0,MPI_COMM_WORLD);
MPI_Send(diffs,3*nBodies,MPI_DOUBLE,1,0,MPI_COMM_WORLD);
MPI_Recv(remotes2,nBodies,MPI_INT,1,0,MPI_COMM_WORLD,MPI_STATUS_IGNORE);
MPI_Recv(diffs2,3*nBodies,MPI_DOUBLE,1,0,MPI_COMM_WORLD,MPI_STATUS_IGNORE);
//apply velocity difference
for(int k=0;k<nBodies;k++)
{
CRigidBody *body = m_pWorld->m_vRigidBodies[remotes2[k]];
//std::cout<<"myid= 0 /id/velocity update from 1: "<<body->m_iID<<" "<<diffs2[3*k+2]<<" "<<body->m_vVelocity;
body->m_vVelocity.x += diffs2[3*k];
body->m_vVelocity.y += diffs2[3*k+1];
body->m_vVelocity.z += diffs2[3*k+2];
//std::cout<<"myid= 0 synced velocity: "<<body->m_vVelocity;
}
delete[] diffs;
delete[] remotes;
delete[] diffs2;
delete[] remotes2;
}
else
{
//max_remotes = max size of int the m_iRemoteBodies vector
//compute diffs
//MPI_Gather(sendStruct,max_remotes, particletype, receiveBuffer, 1, root, GroupComm)
//apply
//MPI_Scatter()
int nBodies = m_pWorld->m_pSubBoundary->m_iRemoteIDs[0].size();
//std::cout<<"Number of remotes in 1 "<<nBodies<<std::endl;
//send struct {diff,targetID}
Real *diffs = new Real[3*nBodies];
int *remotes = new int[nBodies];
Real *diffs2 = new Real[3*nBodies];
int *remotes2 = new int[nBodies];
for(int k=0;k<nBodies;k++)
{
remotes[k]=m_pWorld->m_pSubBoundary->m_iRemoteIDs[0][k];
CRigidBody *body = m_pWorld->m_vRigidBodies[m_pWorld->m_pSubBoundary->m_iRemoteBodies[0][k]];
diffs[3*k] = body->m_vVelocity.x - body->m_vOldVel.x;
diffs[3*k+1] = body->m_vVelocity.y - body->m_vOldVel.y;
diffs[3*k+2] = body->m_vVelocity.z - body->m_vOldVel.z;
//std::cout<<"velocity difference: "<<body->m_vVelocity - body->m_vOldVel;
//std::cout<<"body id/remoteid/velocity: "<<body->m_iID<<" "<<body->m_iRemoteID<<" "<<body->m_vVelocity;
}
MPI_Recv(remotes2,nBodies,MPI_INT,0,0,MPI_COMM_WORLD,MPI_STATUS_IGNORE);
MPI_Recv(diffs2,3*nBodies,MPI_DOUBLE,0,0,MPI_COMM_WORLD,MPI_STATUS_IGNORE);
MPI_Send(remotes,nBodies,MPI_INT,0,0,MPI_COMM_WORLD);
MPI_Send(diffs,3*nBodies,MPI_DOUBLE,0,0,MPI_COMM_WORLD);
for(int k=0;k<nBodies;k++)
{
CRigidBody *body = m_pWorld->m_vRigidBodies[remotes2[k]];
//std::cout<<"myid= 1 /id/velocity update from 0: "<<body->m_iID<<" "<<diffs2[3*k+2]<<" "<<body->m_vVelocity;
body->m_vVelocity.x += diffs2[3*k];
body->m_vVelocity.y += diffs2[3*k+1];
body->m_vVelocity.z += diffs2[3*k+2];
//std::cout<<VECTOR3(diffs2[3*k],diffs2[3*k+1],diffs2[3*k+2]);
//std::cout<<"myid= 1 synced velocity: "<<body->m_vVelocity;
//std::cout<<"myid= 1 /id/velocity update: "<<body->m_iID<<" "<<remotes2[k]<<" "<<diffs2[3*k+2]<<" "<<body->m_vVelocity;
//std::cout<<"myid= 1 /body id/remoteid/velocity: "<<body->m_iID<<" "<<body->m_iRemoteID<<" "<<body->m_vVelocity;
}
for(rIter=vRigidBodies.begin();rIter!=vRigidBodies.end();rIter++)
{
CRigidBody *body = *rIter;
if(body->m_iShape == CRigidBody::BOUNDARYBOX || !body->IsAffectedByGravity())
continue;
VECTOR3 &vel = body->m_vVelocity;
//backup the velocity
body->m_vOldVel = body->m_vVelocity;
//std::cout<<"body id/remoteid/velocity: "<<body->m_iID<<" "<<body->m_iRemoteID<<" "<<body->m_vVelocity;
}//end for
delete[] diffs;
delete[] remotes;
delete[] diffs2;
delete[] remotes2;
}
#endif
//std::cout<<"Iteration: "<<iterations <<" "<<ComputeDefect()<<std::endl;
}
dTimeSolver+=timer0.GetTime();
for(rIter=vRigidBodies.begin();rIter!=vRigidBodies.end();rIter++)
{
RigidBody *body = *rIter;
if(!body->isAffectedByGravity())
continue;
VECTOR3 &vel = body->velocity_;
}//end for
}//end Solve
void CPHMovementControl::ApplyHit(const Fvector& dir,const dReal P,ALife::EHitType hit_type)
{
if(hit_type==ALife::eHitTypeExplosion||hit_type==ALife::eHitTypeWound)
ApplyImpulse(dir,P);
}
void Calculate(int data)
{
glViewport(0, 0, g_iWidth, g_iHeight);
Advect(Velocity.Ping, Velocity.Ping, Boundaries, Velocity.Pong, VelocityDissipation);
SwapSurfaces(&Velocity);
/*Advect(Velocity2.Ping, Velocity2.Ping, Boundaries, Velocity2.Pong, VelocityDissipation);
SwapSurfaces(&Velocity2);*/
Advect(Velocity.Ping, Temperature.Ping, Boundaries, Temperature.Pong, TemperatureDissipation);
SwapSurfaces(&Temperature);
/*Advect(Velocity2.Ping, Temperature2.Ping, Boundaries, Temperature2.Pong, TemperatureDissipation);
SwapSurfaces(&Temperature2);*/
Advect(Velocity.Ping, Density.Ping, Boundaries, Density.Pong, DensityDissipation);
SwapSurfaces(&Density);
/*Advect(Velocity2.Ping, Density2.Ping, Boundaries, Density2.Pong, DensityDissipation);
SwapSurfaces(&Density2);*/
glutMouseFunc(Mouse);
ApplyBuoyancy(Velocity.Ping, Temperature.Ping, Density.Ping, Velocity.Pong, ax, ay);
SwapSurfaces(&Velocity);
/*ApplyBuoyancy2(Velocity.Ping, Temperature.Ping, Density.Ping, Velocity.Pong);
SwapSurfaces(&Velocity);*/
ApplyImpulse(Temperature.Ping, ImpulsePosition, ImpulseTemperature);
ApplyImpulse(Density.Ping, ImpulsePosition, ImpulseDensity);
/*ApplyImpulse(Temperature2.Ping, ImpulsePosition2, ImpulseTemperature);
ApplyImpulse(Density2.Ping, ImpulsePosition2, ImpulseDensity);*/
ComputeDivergence(Velocity.Ping, Boundaries, Divergence);
ClearSurface(Pressure.Ping, 0);
/*ComputeDivergence(Velocity2.Ping, Boundaries, Divergence2);
ClearSurface(Pressure2.Ping, 0);*/
for (int i = 0; i < NumJacobiIterations; ++i) {
Jacobi(Pressure.Ping, Divergence, Boundaries, Pressure.Pong);
SwapSurfaces(&Pressure);
}
/*for (int i = 0; i < NumJacobiIterations; ++i) {
Jacobi(Pressure2.Ping, Divergence2, Boundaries, Pressure2.Pong);
SwapSurfaces(&Pressure2);
}*/
SubtractGradient(Velocity.Ping, Pressure.Ping, Boundaries, Velocity.Pong);
SwapSurfaces(&Velocity);
/*SubtractGradient(Velocity2.Ping, Pressure2.Ping, Boundaries, Velocity2.Pong);
SwapSurfaces(&Velocity2);*/
glutPostRedisplay();
glutTimerFunc(30, Calculate, 1);
}
// greebo: The default implementation of PropagateImpulse just applies the impulse
bool idPhysics_Base::PropagateImpulse( const int id, const idVec3& point, const idVec3& impulse ) {
ApplyImpulse(id, point, impulse);
return false;
}
