void World::Step( float dt )
{
float inv_dt = dt > 0.0f ? 1.0f / dt : 0.0f;
if ( TimingFunction ) { BroadPhaseTime = TimingFunction(); }
BroadPhase();
if ( TimingFunction ) { BroadPhaseTime = TimingFunction() - BroadPhaseTime; }
for ( int i = 0; i < ( int )bodies.size(); ++i ) // forces
{
Body* b = bodies[i];
if ( b->invMass == 0.0f ) { continue; }
b->velocity += dt * ( gravity + b->invMass * b->force );
b->angularVelocity += dt * b->invI * b->torque;
}
if ( TimingFunction ) { PreStepTime = TimingFunction(); }
// pre-steps.
for ( ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb ) { arb->second.PreStep( inv_dt ); }
for ( int i = 0; i < ( int )joints.size(); ++i ) { joints[i]->PreStep( inv_dt ); }
if ( TimingFunction ) { PreStepTime = TimingFunction() - PreStepTime; }
if ( TimingFunction ) { CollisionResolveTime = TimingFunction(); }
for ( int i = 0; i < iterations; ++i ) // iterations
{
for ( ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb ) { arb->second.ApplyImpulse(); }
for ( int j = 0; j < ( int )joints.size(); ++j ) { joints[j]->ApplyImpulse(); }
}
if ( TimingFunction ) { CollisionResolveTime = TimingFunction() - CollisionResolveTime; }
if ( TimingFunction ) { VelocityStepTime = TimingFunction(); }
for ( int i = 0; i < ( int )bodies.size(); ++i ) // velocity step
{
Body* b = bodies[i];
b->position += dt * b->velocity;
b->rotation += dt * b->angularVelocity;
b->force.Set( 0.0f, 0.0f );
b->torque = 0.0f;
}
if ( TimingFunction ) { VelocityStepTime = TimingFunction() - VelocityStepTime; }
}
void World::Update(f64 dt)
{
if(!alive)
return;
static float t_time = 0, f_time=0;
static float owner_update_time = 0;
PerfTimer pt;
PerfTimer ot=pt;
ot.start();
dt=0.016f;
bodies.clear();
if(mouseDown)
{
float sk=550;
if(jointedBody)
{
SimBody *p = jointedBody;
springForce = (float2(mx,my)-p->position)*dt*sk;
p->velocity += springForce;
p->velocity = p->velocity * 0.999f;
}
}
if(netController->mode & NetworkController::Connected)
{
// everyone owns the walls
bodies.push_back(objects[0]);
bodies.push_back(objects[1]);
bodies.push_back(objects[2]);
bodies.push_back(objects[3]);
for(unsigned int i=4;i<objects.size();++i)
{
if(objects[i]->owner == SimBody::whoami)
{
bodies.push_back(objects[i]);
}
}
}
else
{
bodies = objects;
}
// transform vertices into new positions (for every object we own)
for(unsigned int i=0;i<objects.size();++i)
{
objects[i]->UpdateWorldSpaceProperties();
}
BroadPhase();
double inv_dt = 1.0f/dt;
for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb)
{
arb->second.PreStep((f32)inv_dt);
}
for (u32 i = 0; i < bodies.size(); ++i)
{
SimBody *b = bodies[i];
if(b->invMass != 0)
{
b->velocity += (f32)dt * (gravity + b->invMass * b->force);
b->angularVelocity += (f32)dt * b->invI * b->torque;
}
}
for (int i = 0; i < 10; ++i)
{
for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb)
{
arb->second.ApplyImpulse(); // this slows down SIGNIFICANTLY when put on multiple threads :(
}
}
for (u32 i = 0; i < bodies.size(); ++i)
{
SimBody *b = bodies[i];
if(fabs(b->velocity.length_squared()) > 1000)
b->velocity = b->velocity.normalize() * 15;
if(fabs(b->angularVelocity) > 300)
b->angularVelocity /= 20;
b->position += (f32)dt * b->velocity;
b->rotation_in_rads += (f32)dt * b->angularVelocity;
b->force.zero();
b->torque = 0;
}
ot.end();
frameTime = ot.time();
//printf("Frame Time: %f\n", frameTime);
t_time += frameTime;
f_time += frameTime;
if(t_time > 0.0f && (netController->mode & NetworkController::Connected)
&& (netController->mode & NetworkController::Simulating))
{
t_time = 0;
int sentCount=0;
static vector<OwnershipUpdatePacket> opacks;
opacks.clear();
if(doOwnershipUpdates)
{
/***************************************/
/******* OBJECT MIGRATION PACKETS ******/
/***************************************/
static vector<ObjectBlob> sortedBodies;
sortedBodies.clear();
for(int i=4;i<objects.size();++i)
{
ObjectBlob b;
b.b = objects[i];
b.index=i;
sortedBodies.push_back(b);
}
sort(sortedBodies);
ObjectBlob *mid = &sortedBodies[sortedBodies.size()/2];
vector<ObjectBlob*> onLeft, onRight;
// make a list of objects we own on the left/right of mid
for(int i=0;i<sortedBodies.size();++i)
{
ObjectBlob *blob = &sortedBodies[i];
if(blob->b->owner != SimBody::whoami) continue;
if(blob->b->position.x <= mid->b->position.x) onLeft.push_back(blob);
else onRight.push_back(blob);
}
vector<ObjectBlob*> &listToSend = onLeft.size()>=onRight.size() ? onRight : onLeft;
for(int i=0;i<listToSend.size();++i)
{
OwnershipUpdatePacket op;
ObjectBlob &b = *listToSend[i];
op.Prepare(b.index, b.b->velocity, b.b->angularVelocity);
b.b->owner = b.b->owner == 1 ? 2 : 1;
opacks.push_back(op);
}
}
/**************************************/
/********** UPDATE PACKETS ************/
/**************************************/
static vector<PositionOrientationUpdatePacket> updatePacks;
updatePacks.clear();
for(int j=4;j<objects.size();++j)
{
if(objects[j]->owner == SimBody::whoami)
{
if(!close(objects[j]->position, objects[j]->last_pos_sent))
{
PositionOrientationUpdatePacket pop;
pop.Prepare(j, objects[j]->position, objects[j]->rotation_in_rads);
updatePacks.push_back(pop);
objects[j]->last_pos_sent = objects[j]->position;
++sentCount;
}
}
}
/*********** SEND CAMERA UPDATE ***********/
CameraUpdatePacket cap;
cap.Prepare(mypvr.bl, mypvr.tr);
int amountSent = 0;
while(amountSent < sizeof(cap))
{
amountSent=0;
for(int i=0;i<netController->peers.size();++i)
{
amountSent += send(netController->peers[i].socket, (char*)&cap, sizeof(cap),0);
if(amountSent == -1)
{
amountSent = sizeof(cap);
break;
}
}
}
/*************** SEND POSITION AND ORIENTATION UPDATES ***********/
if(updatePacks.size())
{
int dataSize = sizeof(PositionOrientationUpdatePacket)*updatePacks.size();
for(int i=0;i<netController->peers.size();++i)
{
amountSent = 0;
//cout << "Update pack size: " << updatePacks.size() << endl;
while(amountSent < dataSize)
{
amountSent += send(netController->peers[i].socket, (char*)(&updatePacks[0]), dataSize, 0);
if(amountSent == -1)
{
amountSent = dataSize;
break;
}
}
}
}
/*********** SEND OBJECT MIGRATION UPDATES ****************/
if(opacks.size())
{
int ownershipDataSize = sizeof(OwnershipUpdatePacket)*opacks.size();
int ownershipAmountSent = 0;
for(int i=0;i<netController->peers.size();++i)
{
ownershipAmountSent = 0;
while(ownershipAmountSent < ownershipDataSize)
{
ownershipAmountSent += send(netController->peers[i].socket, (char*)(&opacks[0]), ownershipDataSize, 0);
if(ownershipAmountSent == -1)
{
ownershipAmountSent = ownershipDataSize;
break;
}
}
}
}
numberOfObjectsSent = sentCount;
} // end of if(t_time>0 ...)
};
