void WorldSimulation::Advance(Real dt)
{
if(fakeSimulation) {
AdvanceFake(dt);
return;
}
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
Reset(i->second);
}
Timer timer;
Real timeLeft=dt;
Real accumTime=0;
int numSteps = 0;
//printf("Advance %g -> %g\n",time,time+dt);
while(timeLeft > 0.0) {
Real step = Min(timeLeft,simStep);
for(size_t i=0;i<controlSimulators.size();i++)
controlSimulators[i].Step(step);
for(size_t i=0;i<hooks.size();i++)
hooks[i]->Step(step);
odesim.Step(step);
accumTime += step;
timeLeft -= step;
numSteps++;
//accumulate contact information
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->second.accum || i->second.accumFull) {
ODEContactList* list = odesim.GetContactFeedback(i->first.first,i->first.second);
assert(list);
if(i->second.accum) {
if(list->forces.empty()) i->second.hadSeparation = true;
else i->second.hadContact = true;
for(size_t k=0;k<list->forces.size();k++) {
i->second.meanForce += list->forces[k];
i->second.meanPoint += list->points[k].x*(1.0/list->forces.size());
}
}
if(i->second.accumFull) {
i->second.times.push_back(time + accumTime);
i->second.contactLists.push_back(*list);
}
}
}
}
time += dt;
UpdateModel();
//convert sums to means
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->second.accum) {
i->second.meanForce /= numSteps;
i->second.meanPoint /= numSteps;
}
}
//printf("WorldSimulation: Sim step %gs, real step %gs\n",dt,timer.ElapsedTime());
}
void WorldSimulation::Advance(Real dt)
{
if(fakeSimulation) {
AdvanceFake(dt);
return;
}
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
Reset(i->second);
}
Timer timer;
Real timeLeft=dt;
Real accumTime=0;
int numSteps = 0;
//printf("Advance %g -> %g\n",time,time+dt);
while(timeLeft > 0.0) {
Real step = Min(timeLeft,simStep);
for(size_t i=0;i<controlSimulators.size();i++)
controlSimulators[i].Step(step);
for(size_t i=0;i<hooks.size();i++)
hooks[i]->Step(step);
//update viscous friction approximation as dry friction from current velocity
for(size_t i=0;i<controlSimulators.size();i++) {
Robot* robot=world->robots[i].robot;
for(size_t j=0;j<robot->drivers.size();j++) {
//setup viscous friction
if(robot->drivers[j].viscousFriction != 0) {
Real v=controlSimulators[i].oderobot->GetDriverVelocity(j);
for(size_t k=0;k<robot->drivers[j].linkIndices.size();k++) {
int l=robot->drivers[j].linkIndices[k];
controlSimulators[i].oderobot->SetLinkDryFriction(l,robot->drivers[j].dryFriction+robot->drivers[j].viscousFriction*Abs(v));
}
}
}
}
odesim.Step(step);
accumTime += step;
timeLeft -= step;
numSteps++;
//accumulate contact information
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->second.accum || i->second.accumFull) {
ODEContactList* list = odesim.GetContactFeedback(i->first.first,i->first.second);
assert(list);
if(i->second.accum) {
if(list->forces.empty()) i->second.hadSeparation = true;
else i->second.hadContact = true;
for(size_t k=0;k<list->forces.size();k++) {
i->second.meanForce += list->forces[k];
i->second.meanPoint += list->points[k].x*(1.0/list->forces.size());
}
}
if(i->second.accumFull) {
i->second.times.push_back(time + accumTime);
i->second.contactLists.push_back(*list);
}
}
}
}
time += dt;
UpdateModel();
//convert sums to means
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->second.accum) {
i->second.meanForce /= numSteps;
i->second.meanPoint /= numSteps;
}
}
//printf("WorldSimulation: Sim step %gs, real step %gs\n",dt,timer.ElapsedTime());
}
void WorldSimulation::Advance(Real dt)
{
if(fakeSimulation) {
AdvanceFake(dt);
return;
}
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
Reset(i->second);
}
Timer timer;
Real timeLeft=dt;
Real accumTime=0;
int numSteps = 0;
//printf("Advance %g -> %g\n",time,time+dt);
while(timeLeft > 0.0) {
Real step = Min(timeLeft,simStep);
for(size_t i=0;i<controlSimulators.size();i++)
controlSimulators[i].Step(step);
for(size_t i=0;i<hooks.size();i++)
hooks[i]->Step(step);
//update viscous friction approximation as dry friction from current velocity
for(size_t i=0;i<controlSimulators.size();i++) {
Robot* robot=world->robots[i].robot;
for(size_t j=0;j<robot->drivers.size();j++) {
//setup viscous friction
if(robot->drivers[j].viscousFriction != 0) {
Real v=controlSimulators[i].oderobot->GetDriverVelocity(j);
for(size_t k=0;k<robot->drivers[j].linkIndices.size();k++) {
int l=robot->drivers[j].linkIndices[k];
controlSimulators[i].oderobot->SetLinkDryFriction(l,robot->drivers[j].dryFriction+robot->drivers[j].viscousFriction*Abs(v));
}
}
}
}
odesim.Step(step);
accumTime += step;
timeLeft -= step;
numSteps++;
//accumulate contact information
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->second.accum || i->second.accumFull) {
ODEContactList* list = odesim.GetContactFeedback(i->first.first,i->first.second);
if(!list) continue;
if(i->second.accum) {
if(list->forces.empty()) i->second.separationCount++;
else i->second.contactCount++;
i->second.inContact = !list->forces.empty();
Vector3 meanPoint(Zero),meanForce(Zero),meanTorque(Zero);
if(!list->forces.empty()) {
Real wsum = 0;
for(size_t k=0;k<list->forces.size();k++) {
Real w = list->forces[k].dot(list->points[k].n);
meanPoint += list->points[k].x*w;
wsum += w;
}
if(wsum == 0) {
meanPoint.setZero();
for(size_t k=0;k<list->forces.size();k++)
meanPoint += list->points[k].x;
meanPoint /= list->forces.size();
}
else
meanPoint /= wsum;
//update average;
i->second.meanPoint += 1.0/i->second.contactCount*(meanPoint - i->second.meanPoint);
}
for(size_t k=0;k<list->forces.size();k++) {
meanForce += list->forces[k];
meanTorque += cross((list->points[k].x-meanPoint),list->forces[k]);
}
//update average
i->second.meanForce += 1.0/numSteps*(meanForce - i->second.meanForce);
i->second.meanTorque += 1.0/numSteps*(meanTorque - i->second.meanTorque);
}
if(i->second.accumFull) {
i->second.times.push_back(time + accumTime);
i->second.contactLists.push_back(*list);
}
}
}
}
time += dt;
UpdateModel();
/*
//convert sums to means
for(ContactFeedbackMap::iterator i=contactFeedback.begin();i!=contactFeedback.end();i++) {
if(i->second.accum) {
i->second.meanForce /= numSteps;
i->second.meanPoint /= numSteps;
i->second.meanTorque /= numSteps;
}
}
*/
//printf("WorldSimulation: Sim step %gs, real step %gs\n",dt,timer.ElapsedTime());
}
