void State::Update(double deltaTime) {
if(IsActive() == false) {
ClearImpulses();
ClearForces();
SetVelocity(0.0, 0.0);
SetAcceleration(0.0, 0.0);
return;
}
a2de::Vector2D total_forces;
std::for_each(_forces.begin(), _forces.end(), [&total_forces] (ForceContainer::value_type& current_force) mutable -> a2de::Vector2D
{
if(current_force.second < 0.0) return Vector2D();
total_forces += current_force.first;
return total_forces;
});
Vector2D total_impulses = std::accumulate(_impulses.begin(), _impulses.end(), Vector2D());
Vector2D F(total_forces + total_impulses);
ClearImpulses();
double mass = GetMass();
//If static body, do nothing.
if(Math::IsEqual(mass, 0.0)) {
ClearImpulses();
ClearForces();
SetVelocity(0.0, 0.0);
SetAcceleration(0.0, 0.0);
Sleep();
return;
}
//Integrate from constant acceleration.
//a = F / m
//v = at + v;
//p = (1/2)at^2 + vt + p
SetAcceleration(F / mass);
SetVelocity(GetAcceleration() * deltaTime + GetVelocity());
SetPosition(((0.5 * GetAcceleration()) * deltaTime * deltaTime) + (GetVelocity() * deltaTime) + GetPosition());
_forces.remove_if([=](ForceContainer::value_type& current_force)->bool {
current_force.second -= deltaTime;
return (current_force.second < 0.0);
});
if(_impulses.empty() && _forces.empty() &&
((a2de::Math::IsEqual(_acceleration.GetX(), 0.0) && a2de::Math::IsEqual(_acceleration.GetY(), 0.0)) &&
(a2de::Math::IsEqual(_velocity.GetX(), 0.0) && a2de::Math::IsEqual(_velocity.GetY(), 0.0))))
{
SetVelocity(0.0, 0.0);
SetAcceleration(0.0, 0.0);
Sleep();
} else {
Wake();
}
}
int IForce_Init(int port)
{
Bool result;
if (iForceInit) return 1;
if (port < 1 || port > 4) return 0;
SetJoystickPort(port);
result = InitStick(&iForceCaps);
if (!result) {
logentry( "IFORCE: Initialization failed!.\n");
return 0;
}
if (!EnableForces()) {
logentry("IFORCE: Unable to enable forces.\n");
return 0;
}
ClearForces();
iForceInit = TRUE;
logentry("IFORCE: Initialization complete.\n");
return 1;
}
void b3World::Step(const b3TimeStep& step) {
b3Time stepTime;
if (m_flags & e_bodyAddedFlag) {
// If new shapes were added we need to find new contacts.
m_contactGraph.FindNewContacts();
m_flags &= ~b3World::e_bodyAddedFlag;
}
{
b3Time time;
// Update contact constraints. Destroy ones if they aren't intersecting.
m_contactGraph.UpdateContacts();
time.Update();
m_profile.narrowPhaseTime = time.GetDeltaMilisecs();
}
{
b3Time time;
// Solve system's EDOs and MLCPs.
Solve(step);
time.Update();
m_profile.solverTime = time.GetDeltaMilisecs();
}
if (m_flags & e_clearForcesFlag) {
ClearForces();
}
stepTime.Update();
m_profile.totalTime = stepTime.GetDeltaMilisecs();
}
void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
{
// If new fixtures were added, we need to find the new contacts.
if (m_flags & e_newFixture)
{
m_contactManager.FindNewContacts();
m_flags &= ~e_newFixture;
}
m_flags |= e_locked;
b2TimeStep step;
step.dt = dt;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
m_contactManager.Collide();
// Integrate velocities, solve velocity constraints, and integrate positions.
if (step.dt > 0.0f)
{
Solve(step);
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
SolveTOI();
}
if (step.dt > 0.0f)
{
m_inv_dt0 = step.inv_dt;
}
if (m_flags & e_clearForces)
{
ClearForces();
}
m_flags &= ~e_locked;
}
void StableWorld::Step(float32 timeStep,
int32 velocityIterations,
int32 positionIterations)
{
elapsed_ += timeStep;
const int steps = static_cast<int>(elapsed_ * kStepsPerMs);
if (steps == 0)
RestoreState();
else
{
elapsed_ -= steps / kStepsPerMs;
for (int i = 0; i < std::min(steps, kMaxSteps); ++i)
{
b2World::Step(
kFixedStep, velocityIterations, positionIterations);
SaveState();
}
ClearForces(); // TODO: Needed?
Interpolate();
}
}
void CSheetSimulator::EulerStep( float dt )
{
ClearForces();
ComputeForces();
// Update positions and velocities
for ( int i = 0; i < NumParticles(); ++i)
{
m_Particle[i].m_Position += m_Particle[i].m_Velocity * dt;
m_Particle[i].m_Velocity += m_Particle[i].m_Force * dt / m_Particle[i].m_Mass;
assert( _finite( m_Particle[i].m_Velocity.x ) &&
_finite( m_Particle[i].m_Velocity.y) &&
_finite( m_Particle[i].m_Velocity.z) );
// clamp for stability
float lensq = m_Particle[i].m_Velocity.LengthSqr();
if (lensq > 1e6)
{
m_Particle[i].m_Velocity *= 1e3 / sqrt(lensq);
}
}
SatisfyCollisionConstraints();
}
void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
{
b2Timer stepTimer;
memset(&m_profile, 0, sizeof(m_profile));
// If new fixtures were added, we need to find the new contacts.
if (m_flags & e_newFixture)
{
b2Timer timer;
if (IsMultithreadedStepEnabled())
{
FindNewContactsMT();
}
else
{
m_contactManager.FindNewContacts(0, m_contactManager.m_broadPhase.GetMoveCount());
}
m_flags &= ~e_newFixture;
float32 elapsed = timer.GetMilliseconds();
m_profile.broadphase += elapsed;
m_profile.broadphaseFindContacts += elapsed;
}
m_flags |= e_locked;
b2TimeStep step;
step.dt = dt;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
{
b2Timer timer;
if (IsMultithreadedStepEnabled())
{
CollideMT();
}
else
{
m_contactManager.Collide(m_contactManager.m_contactsNonTOI.Data(), m_contactManager.m_contactsNonTOI.GetCount());
m_contactManager.Collide(m_contactManager.m_contactsTOI.Data(), m_contactManager.m_contactsTOI.GetCount());
}
m_profile.collide += timer.GetMilliseconds();
}
// Integrate velocities, solve velocity constraints, and integrate positions.
if (m_stepComplete && step.dt > 0.0f)
{
b2Timer timer;
if (IsMultithreadedStepEnabled())
{
SolveMT(step);
}
else
{
Solve(step);
}
m_profile.solve += timer.GetMilliseconds();
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
b2Timer timer;
SolveTOI(step);
m_profile.solveTOI += timer.GetMilliseconds();
}
if (step.dt > 0.0f)
{
m_inv_dt0 = step.inv_dt;
}
if (m_flags & e_clearForces)
{
ClearForces();
}
m_flags &= ~e_locked;
m_profile.step = stepTimer.GetMilliseconds();
}
void b2World::Step(
float32 dt,
int32 velocityIterations,
int32 positionIterations,
int32 particleIterations)
{
b2Timer stepTimer;
// If new fixtures were added, we need to find the new contacts.
if (m_flags & e_newFixture)
{
m_contactManager.FindNewContacts();
m_flags &= ~e_newFixture;
}
m_flags |= e_locked;
b2TimeStep step;
step.dt = dt;
step.velocityIterations = velocityIterations;
step.positionIterations = positionIterations;
step.particleIterations = particleIterations;
if (dt > 0.0f)
{
step.inv_dt = 1.0f / dt;
}
else
{
step.inv_dt = 0.0f;
}
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
// Update contacts. This is where some contacts are destroyed.
{
b2Timer timer;
m_contactManager.Collide();
m_profile.collide = timer.GetMilliseconds();
}
// Integrate velocities, solve velocity constraints, and integrate positions.
if (m_stepComplete && step.dt > 0.0f)
{
b2Timer timer;
for (b2ParticleSystem* p = m_particleSystemList; p; p = p->GetNext())
{
p->Solve(step); // Particle Simulation
}
Solve(step);
m_profile.solve = timer.GetMilliseconds();
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
b2Timer timer;
SolveTOI(step);
m_profile.solveTOI = timer.GetMilliseconds();
}
if (step.dt > 0.0f)
{
m_inv_dt0 = step.inv_dt;
}
if (m_flags & e_clearForces)
{
ClearForces();
}
m_flags &= ~e_locked;
m_profile.step = stepTimer.GetMilliseconds();
}
void DeadPlayerFrame (void)
{
fix xTimeDead, h;
CFixVector fVec;
if (gameStates.app.bPlayerIsDead) {
xTimeDead = gameData.time.xGame - gameStates.app.nPlayerTimeOfDeath;
// If unable to create camera at time of death, create now.
if (!gameData.objs.deadPlayerCamera) {
CObject *playerP = OBJECTS + LOCALPLAYER.nObject;
int nObject = CreateCamera (playerP);
if (nObject != -1)
gameData.objs.viewerP = gameData.objs.deadPlayerCamera = OBJECTS + nObject;
else
Int3 ();
}
h = DEATH_SEQUENCE_EXPLODE_TIME - xTimeDead;
h = max (0, h);
gameData.objs.consoleP->mType.physInfo.rotVel = CFixVector::Create(h / 4, h / 2, h / 3);
xCameraToPlayerDistGoal = min (xTimeDead * 8, I2X (20)) + gameData.objs.consoleP->info.xSize;
SetCameraPos (&gameData.objs.deadPlayerCamera->info.position.vPos, gameData.objs.consoleP);
fVec = gameData.objs.consoleP->info.position.vPos - gameData.objs.deadPlayerCamera->info.position.vPos;
gameData.objs.deadPlayerCamera->info.position.mOrient = CFixMatrix::CreateF(fVec);
if (xTimeDead > DEATH_SEQUENCE_EXPLODE_TIME) {
if (!gameStates.app.bPlayerExploded) {
if (LOCALPLAYER.hostages.nOnBoard > 1)
HUDInitMessage (TXT_SHIP_DESTROYED_2, LOCALPLAYER.hostages.nOnBoard);
else if (LOCALPLAYER.hostages.nOnBoard == 1)
HUDInitMessage (TXT_SHIP_DESTROYED_1);
else
HUDInitMessage (TXT_SHIP_DESTROYED_0);
#ifdef TACTILE
if (TactileStick)
ClearForces ();
#endif
gameStates.app.bPlayerExploded = 1;
if (gameData.app.nGameMode & GM_NETWORK) {
AdjustMineSpawn ();
MultiCapObjects ();
}
DropPlayerEggs (gameData.objs.consoleP);
gameStates.app.bPlayerEggsDropped = 1;
if (IsMultiGame)
MultiSendPlayerExplode (MULTI_PLAYER_EXPLODE);
gameData.objs.consoleP->ExplodeBadassPlayer ();
//is this next line needed, given the badass call above?
gameData.objs.consoleP->Explode (0);
gameData.objs.consoleP->info.nFlags &= ~OF_SHOULD_BE_DEAD; //don't really kill CPlayerData
gameData.objs.consoleP->info.renderType = RT_NONE; //..just make him disappear
gameData.objs.consoleP->SetType (OBJ_GHOST); //..and kill intersections
LOCALPLAYER.flags &= ~PLAYER_FLAGS_HEADLIGHT_ON;
#if 0
if (gameOpts->gameplay.bFastRespawn)
gameStates.app.bDeathSequenceAborted = 1;
#endif
}
}
else {
if (d_rand () < gameData.time.xFrame * 4) {
if (gameData.app.nGameMode & GM_MULTI)
MultiSendCreateExplosion (gameData.multiplayer.nLocalPlayer);
CreateSmallFireballOnObject (gameData.objs.consoleP, I2X (1), 1);
}
}
if (gameStates.app.bDeathSequenceAborted) { //xTimeDead > DEATH_SEQUENCE_LENGTH) {
StopPlayerMovement ();
gameStates.app.bEnterGame = 2;
if (!gameStates.app.bPlayerEggsDropped) {
if (gameData.app.nGameMode & GM_NETWORK) {
AdjustMineSpawn ();
MultiCapObjects ();
}
DropPlayerEggs (gameData.objs.consoleP);
gameStates.app.bPlayerEggsDropped = 1;
if (gameData.app.nGameMode & GM_MULTI)
MultiSendPlayerExplode (MULTI_PLAYER_EXPLODE);
}
DoPlayerDead (); //kill_playerP ();
}
}
}
int main(int argc, char** argv) {
srand((unsigned int)time(NULL));
// Calculate the length of the box side
double length = powf(N/density, 1.0/3.0);
// Set boundaries
for (int k = 0; k < 3; ++k) {
L[k] = length;
L2[k] = L[k]/2.0;
if (rcut2 > 0) {
assert(L[k] > sqrt(rcut2));
}
}
// Set masses for every particle
for (int i = 0; i < N; ++i) {
m[i] = mAr;
}
FILE* f_en;
FILE* f_kin;
FILE* f_poten;
FILE* f_temp;
FILE* f_xyz;
FILE* f_velocity;
FILE* f_init_coord;
FILE* f_len;
if (PRINT_TO_FILE) {
f_init_coord = fopen("data/init_coord.xyz", "w");
f_xyz = fopen("data/coordinates.xyz", "w");
f_temp = fopen("data/temp.csv", "w");
f_velocity = fopen("data/velocity.csv", "w");
f_en = fopen("data/energy.csv", "w");
f_kin = fopen("data/kinetic.csv", "w");
f_poten = fopen("data/poten.csv", "w");
f_len = fopen("data/len.csv", "w");
}
// Store length to file
fprintf(f_len, "%f", length);
/* Set Initial Coordinates */
double init[3];
// Quantity of atoms per line
int quant = powf(N, 1.0/3.0);
// Step of the mesh
double step = L[0] / quant;
double ic = (step/2.0) - L2[0];
for (int i = 0; i < 3; ++i) {
init[i] = ic;
}
// Set initial coordinates
for (int i = 0; i < N; ++i) {
for (int k = 0; k < 3; ++k) {
r[i][k] = init[k];
}
init[0] += step;
if ((i+1) % (quant*quant) == 0) {
init[0] = ic;
init[1] = ic;
init[2] += step;
} else if ((i+1) % quant == 0) {
init[0] = ic;
init[1] += step;
}
}
// Set initial velocities
for (int i = 0; i < N; ++i) {
for (int k = 0; k < 3; ++k) {
v[i][k] = 1 - (((float)rand()/(float)RAND_MAX)*2);
}
}
if (PRINT_TO_FILE) {
// Print initial coordinates to file
fprintf(f_init_coord, "%d\n\n", N);
for (int i = 0; i < N; ++i) {
fprintf(f_init_coord, "%c ", (char) (97+(i%26)));
for (int k = 0; k < 3; ++k) {
fprintf(f_init_coord, "%f ", r[i][k]);
}
fprintf(f_init_coord, "\n");
}
fclose(f_init_coord);
}
///////////////
// Main Part //
///////////////
for (int i = 0; i < iterations; ++i) {
nearest_image();
ClearForces();
CalcForces();
EqMotion();
CalcEnergy();
CalcTemp();
Thermostat(i);
if (PRINT_TO_FILE) {
// Store energy and temperature to file
fprintf(f_en, "%f,%i\n", K + utot, i);
fprintf(f_kin, "%f,%i\n", K, i);
fprintf(f_poten, "%f,%i\n", utot, i);
fprintf(f_temp, "%f,%i\n", Temp, i);
// Store velocities to file
if (i > iterations/2) {
for (int j = 0; j < N; ++j) {
double v2 = 0;
for (int k = 0; k < 3; ++k) {
v2 += (float)v[j][k] * (float)v[j][k];
}
fprintf(f_velocity, "%f\n", sqrt(v2));
}
}
// Store coordinates to file
if (i < iter_to_write) {
fprintf(f_xyz, "%d\n\n", N);
for (int i = 0; i < N; ++i) {
fprintf(f_xyz, "%c ", (char) (97+(i%26)));
for (int k = 0; k < 3; ++k) {
fprintf(f_xyz, "%f ", rn[i][k]);
}
fprintf(f_xyz, "\n");
}
}
}
// Print to console
if (i % (iterations / 10) == 0) {
printf("> iter: %d\n", i);
printf("-> temp: %f\n", Temp);
printf("-> utot: %f\n", utot);
printf("-> K: %f\n", K);
printf("\n");
}
assert(K != INFINITY);
}
if (PRINT_TO_FILE) {
fclose(f_velocity);
fclose(f_xyz);
fclose(f_en);
fclose(f_poten);
fclose(f_kin);
fclose(f_temp);
fclose(f_len);
printf("Print to file: Done!\n");
}
printf("Result: Done!\n");
return 0;
}
