void DESolver::Setup( double *min, double *max, int deStrategy, double diffScale,
double crossoverProb, double ftol, unsigned long rngSeed )
{
int i;
strategy = deStrategy;
scale = diffScale;
probability = crossoverProb;
tolerance = ftol;
// PE: seed the (Mersenne Twister) RNG
if (rngSeed > 0)
init_genrand(rngSeed);
else
init_genrand((unsigned long)time((time_t *)NULL));
CopyVector(minBounds, min);
CopyVector(maxBounds, max);
for (i = 0; i < nPop; i++) {
for (int j = 0; j < nDim; j++)
Element(population,i,j) = RandomUniform(min[j], max[j]);
popEnergy[i] = 1.0E20;
}
for (i = 0; i < nDim; i++)
bestSolution[i] = 0.0;
}
//assumes normalized normals
bool operator () (const dContactGeom& a,const dContactGeom& b)
{
Vector3 ax,bx;
CopyVector(ax,a.pos);
CopyVector(bx,b.pos);
return ax.distanceSquared(bx) < Sqr(ptol);
}
void HouseTypeExt::ExtData::InheritSettings(HouseTypeClass *pThis) {
if(auto ParentCountry = HouseTypeClass::Find(pThis->ParentCountry)) {
if(const auto ParentData = HouseTypeExt::ExtMap.Find(ParentCountry)) {
CopyString(&HouseTypeExt::ExtData::FlagFile, ParentData, this);
CopyString(&HouseTypeExt::ExtData::ObserverFlag, ParentData, this);
CopyString(&HouseTypeExt::ExtData::ObserverBackground, ParentData, this);
CopyString(&HouseTypeExt::ExtData::LSFile, ParentData, this);
CopyString(&HouseTypeExt::ExtData::LSPALFile, ParentData, this);
CopyString(&HouseTypeExt::ExtData::TauntFile, ParentData, this);
CopyString(&HouseTypeExt::ExtData::LSName, ParentData, this);
CopyString(&HouseTypeExt::ExtData::LSSpecialName, ParentData, this);
CopyString(&HouseTypeExt::ExtData::LSBrief, ParentData, this);
CopyString(&HouseTypeExt::ExtData::StatusText, ParentData, this);
this->LoadTextColor = ParentData->LoadTextColor;
this->RandomSelectionWeight = ParentData->RandomSelectionWeight;
this->CountryListIndex = ParentData->CountryListIndex + 1;
this->ObserverBackgroundSHP = ParentData->ObserverBackgroundSHP;
this->ObserverFlagSHP = ParentData->ObserverFlagSHP;
this->ObserverFlagYuriPAL = ParentData->ObserverFlagYuriPAL;
this->ParaDropPlane.Set(ParentData->ParaDropPlane);
this->Parachute_Anim.Set(ParentData->Parachute_Anim);
CopyVector(&HouseTypeExt::ExtData::Powerplants, ParentData, this);
CopyVector(&HouseTypeExt::ExtData::ParaDrop, ParentData, this);
CopyVector(&HouseTypeExt::ExtData::ParaDropNum, ParentData, this);
CopyStdVector(&HouseTypeExt::ExtData::VeteranBuildings, ParentData, this);
}
}
this->SettingsInherited = true;
}
/* SetCovs: set covariance values in hmm */
void SetCovs(void)
{
int i,s,m;
StateElem *se;
StreamElem *ste;
MixtureElem *me;
MixPDF *mp;
CalcCovs();
if (trace&T_TOP) {
printf("Updating HMM ");
if (meanUpdate) printf("Means and ");
printf("Covariances\n");
}
for (i=2,se=hmmLink->svec+2; i < hmmLink->numStates; i++,se++)
for (s=1,ste=se->info->pdf+1; s <= hset.swidth[0]; s++,ste++)
for (m=1,me = ste->spdf.cpdf+1; m<=ste->nMix; m++, me++) {
mp = me->mpdf;
if (meanUpdate && !IsSeenV(mp->mean)){ /* meanSum now holds mean */
CopyVector(accs[s].meanSum,mp->mean);
TouchV(mp->mean);
}
if (!IsSeenV(mp->cov.var)){
if (mp->ckind==FULLC)
CopyMatrix(accs[s].fixed.inv,mp->cov.inv);
else if (fullcNeeded[s]) /* dont need full cov, but its all we have */
TriDiag2Vector(accs[s].fixed.inv,mp->cov.var);
else
CopyVector(accs[s].fixed.var,mp->cov.var);
TouchV(mp->cov.var);
}
}
ClearSeenFlags(&hset,CLR_ALL);
}
DexAnalogMixin::DexAnalogMixin( void ) {
Quaternion align, flip;
// Define the rotation of the ATI sensors with respect to the local
// manipulandum reference frame. These are probably constants,
// but perhaps they should be read from the model-specific parameter file as well.
ATIRotationAngle[LEFT_ATI] = LEFT_ATI_ROTATION;
ATIRotationAngle[RIGHT_ATI] = RIGHT_ATI_ROTATION;
// Compute the transformations to put ATI forces in a common reference frame.
// The local manipulandum reference frame should align with
// the world reference frame when the manipulandum is held upright in the seated posture.
SetQuaterniond( ftAlignmentQuaternion[0], ATIRotationAngle[0], kVector );
SetQuaterniond( align, ATIRotationAngle[1], kVector );
SetQuaterniond( flip, 180.0, iVector );
MultiplyQuaternions( ftAlignmentQuaternion[1], flip, align );
// Set a default filter constant.
SetFilterConstant( 100.0 );
// Initialize some instance variables used to hold the current state
// when filtering certain vector values.
CopyVector( filteredManipulandumPosition, zeroVector );
CopyVector( filteredManipulandumRotations, zeroVector );
CopyVector( filteredLoadForce, zeroVector );
CopyVector( filteredAcceleration, zeroVector );
for (int ati = 0; ati < N_FORCE_TRANSDUCERS; ati++ ) CopyVector( filteredCoP[ati], zeroVector );
filteredGripForce = 0.0;
}
///Will produce bogus o1 and o2 vectors
void GetContacts(dBodyID a,vector<ODEContactList>& contacts)
{
if(a == 0) return;
contacts.resize(0);
for(list<ODEContactResult>::iterator i=gContacts.begin();i!=gContacts.end();i++) {
if(a == dGeomGetBody(i->o1) || a == dGeomGetBody(i->o2)) {
dBodyID b = dGeomGetBody(i->o2);
bool reverse = false;
if(b == a) { b = dGeomGetBody(i->o1); reverse = true; }
contacts.resize(contacts.size()+1);
contacts.back().points.resize(i->contacts.size());
contacts.back().forces.resize(i->feedback.size());
for(size_t j=0;j<i->feedback.size();j++) {
CopyVector(contacts.back().forces[j],i->feedback[j].f1);
CopyVector(contacts.back().points[j].x,i->contacts[j].pos);
CopyVector(contacts.back().points[j].n,i->contacts[j].normal);
//contacts.back().points[j].kFriction = i->contacts[j].surface.mu;
contacts.back().points[j].kFriction = 0;
if(reverse) {
contacts.back().forces[j].inplaceNegative();
contacts.back().points[j].n.inplaceNegative();
}
}
}
}
}
bool DESolver::Solve(int maxGenerations)
{
int generation;
int candidate;
bool bAtSolution;
bestEnergy = 1.0E20;
bAtSolution = false;
for (generation=0;(generation < maxGenerations) && !bAtSolution;generation++)
for (candidate=0; candidate < nPop; candidate++)
{
(this->*calcTrialSolution)(candidate);
trialEnergy = EnergyFunction(trialSolution,bAtSolution);
if (trialEnergy < popEnergy[candidate])
{
// New low for this candidate
popEnergy[candidate] = trialEnergy;
CopyVector(RowVector(population,candidate),trialSolution);
// Check if all-time low
if (trialEnergy < bestEnergy)
{
bestEnergy = trialEnergy;
CopyVector(bestSolution,trialSolution);
}
}
}
generations = generation;
return(bAtSolution);
}
Frustrum::Frustrum( double near_v[3], double far_v[3] ) {
CopyVector( near_vertex, near_v );
CopyVector( far_vertex, far_v );
multi_color = true;
OpenGLObject(); // Do what every OpenGlObject does at creation.
}
/** Two ways of merging frictionless contacts without resorting to 6D
* wrench space
* 1) interior points in 2D convex hull, with the projection defined
* by the contact normal
* 2) interior points in convex cone at a single contact point (TODO)
*
* If contacts have friction, the convex hull method only works when the
* points are on the same plane.
*/
void CHContactsPlane(vector<dContactGeom>& contacts)
{
if(contacts.empty()) return;
if(contacts.size() <= 2) return;
Vector3 c(Zero),n(Zero);
for(size_t i=0;i<contacts.size();i++) {
Vector3 cx,nx;
CopyVector(cx,contacts[i].pos);
CopyVector(nx,contacts[i].normal);
c += cx;
n += nx;
}
c /= contacts.size();
n.inplaceNormalize();
//get the deepest contact
size_t deepest = 0;
for(size_t i=1;i<contacts.size();i++)
if(contacts[i].depth > contacts[deepest].depth) deepest=i;
//make a plane
Vector3 x,y;
n.getOrthogonalBasis(x,y);
//Real nofs = n.dot(c);
Real xofs = x.dot(c);
Real yofs = y.dot(c);
vector<Vector2> pt2d(contacts.size());
for(size_t i=0;i<contacts.size();i++) {
Vector3 cx;
CopyVector(cx,contacts[i].pos);
pt2d[i].x = x.dot(cx)-xofs;
pt2d[i].y = y.dot(cx)-yofs;
}
//get the convex hull
vector<Vector2> ch(contacts.size());
vector<int> mapping(contacts.size());
int num=Geometry::ConvexHull2D_Chain_Unsorted(&pt2d[0],contacts.size(),&ch[0],&mapping[0]);
vector<dContactGeom> temp(num);
bool addDeepest = true;
for(int i=0;i<num;i++) {
Assert(mapping[i] < (int)contacts.size());
temp[i] = contacts[mapping[i]];
if(mapping[i] == (int)deepest)
addDeepest = false;
}
if(addDeepest)
temp.push_back(contacts[deepest]);
swap(temp,contacts);
}
//assumes normalized normals
bool operator () (const dContactGeom& a,const dContactGeom& b)
{
Vector3 an,bn;
CopyVector(an,a.normal);
CopyVector(bn,b.normal);
Real ndot = an.dot(bn);
if(ndot > catol) {
Vector3 ax,bx;
CopyVector(ax,a.pos);
CopyVector(bx,b.pos);
return FuzzyEquals(ax.dot(an),bx.dot(an),ptol) &&
FuzzyEquals(ax.dot(bn),bx.dot(bn),ptol);
}
return false;
}
/* PutVFloor: output variance floor vectors */
void PutVFloor(void)
{
int i,s;
char outfn[MAXSTRLEN],vName[32],num[10];
FILE *f;
Vector v;
MakeFN("vFloors",outDir,NULL,outfn);
if ((f = fopen(outfn,"w")) == NULL)
HError(2011,"PutVFloor: cannot create %s",outfn);
for (s=1; s <= hset.swidth[0]; s++) {
v = CreateVector(&gstack,hset.swidth[s]);
sprintf(num,"%d",s);
strcpy(vName,"varFloor"); strcat(vName,num);
fprintf(f,"~v %s\n",vName);
if (fullcNeeded[s])
TriDiag2Vector(accs[s].squareSum.inv,v);
else
CopyVector(accs[s].fixed.var,v);
for (i=1; i<=hset.swidth[s]; i++)
v[i] *= vFloorScale;
fprintf(f,"<Variance> %d\n",hset.swidth[s]);
WriteVector(f,v,FALSE);
FreeVector(&gstack,v);
}
fclose(f);
if (trace&T_TOP)
printf("Var floor macros output to file %s\n",outfn);
}
void ExternalToolsDialog::Apply()
{
// If a tool doesn't have a name, give it one since this prevents
// us from putting it in the menu.
int selectedItem = m_listBox->GetSelection();
for (unsigned int i = 0; i < m_workingTools.size(); ++i)
{
ExternalTool* tool = m_workingTools[i];
if (tool->GetTitle().IsEmpty())
{
tool->SetTitle("Unnamed Tool");
m_listBox->Delete(i);
m_listBox->Insert(tool->GetTitle(), i);
}
}
m_listBox->SetSelection(selectedItem);
UpdateControlsForSelection(selectedItem);
// Apply the changes.
CopyVector(*m_appliedTools, m_workingTools);
}
/* Estimating best value of lambda via minimising the mean squared error
* under bootstraps (see Storey 04, and note that the bootstrapping has
* an analytical solution since W(lambda) is binomial).
* Lambda restricted to [0,max pval)
*/
double estimate_lambda_storey04 ( const double * pval, const unsigned int m ){
assert(NULL!=pval);
/* Sort pvals */
double * pval_sort = CopyVector(pval,malloc(m*sizeof(double)),m);
qsort(pval_sort,m,sizeof(double),CmpDouble);
/* Find pi_star, min_lambda pi_0(lambda) */
double pi_star = 1.;
for ( unsigned int i=0 ; i<m ; i++){
double pi_0 = (double)(m-i-1)/(double)(m*(1.-pval_sort[i]));
if ( pi_0 < pi_star){ pi_star = pi_0; }
}
double lambda_min = 0.;
double mse_min = (1. - pi_star)*(1. - pi_star);
for ( unsigned int i=0 ; i<m-1 ; i++){
const double lambda = pval_sort[i];
const double p = (m-i-1)/(double)m;
const double pi_0 = p/(1.-lambda);
const double mse = p*(1-p)/((1.-lambda)*(1.-lambda))
+ pi_0*pi_0
- 2*pi_star*pi_0
+ pi_star*pi_star;
if (mse<mse_min){
mse_min = mse;
lambda_min = pval_sort[i];
}
}
free(pval_sort);
return lambda_min;
}
/* MakeWtAccLists: Copy info from WtAcc to WALink and add WALink to wtStore,
Zero WtAcc afterwards */
void MakeWtAccLists()
{
int ix,n,s,i,nMix;
HMMScanState hss;
HLink hmm;
WALink *w;
StateElem *se;
StreamElem *ste;
WtAcc *wa;
NewHMMScan(&hset,&hss);
ix=1;
do {
hmm = hss.hmm;
for (i=2,se = hmm->svec+2; i<hmm->numStates;i++,se++)
for (s=1,ste = se->info->pdf+1; s<=nStreams; s++,ste++){
w = &(wtStore[ix][i][s]); n = 0;
while (*w != NULL){
++n; w = &((*w)->next);
}
nMix = (hset.hsKind==TIEDHS) ? hset.tmRecs[s].nMix : ste->nMix;
(*w) = CreateChWtAcc(&wtAccStack, nMix);
wa = (WtAcc *)ste->hook;
CopyVector(wa->c,(*w)->c);
(*w)->occ = wa->occ;
wa->occ = 0;
ZeroVector(wa->c);
}
ix++;
} while (GoNextHMM(&hss));
EndHMMScan(&hss);
}
/****************************************************************************
Assign rotamer to be close to that in PDB, and calcualte cartesian coordinates
*****************************************************************************/
void SetRotamersSimilarToPDB(int nc, struct s_polymer *p, struct atom_s *a, int napdb)
{
int ic,i,ir=0,irotmin,iaa,is,ipdb,found;
struct vector pdb[ROT_ATMAX],cpol[ROT_ATMAX];
double rmsd2min=99999.,x;
fprintf(stderr,"Set rotamers close to those of PDB\n");
for(ic=0;ic<nc;ic++)
for(i=0;i<(p+ic)->nback;i++)
if (!strcmp( (((p+ic)->back)+i)->type , "CA" ) && (((p+ic)->back)+i)->nside>0) // loop on all CA atoms
{
iaa = (((p+ic)->back)+i)->iaa;
// find positions in the pdb
for(is=1;is< (((p+ic)->back)+i)->nside; is++) // loop on sides of polymer, except CB
{
found=0;
for(ipdb=0;ipdb<napdb;ipdb++)
{
if ( (a+ipdb)->chain == ic && (a+ipdb)->iaa == iaa &&
!strcmp((a+ipdb)->atom,((((((p+ic)->back)+i)->side)+is)->type) ) )
{ CopyVector(&((a+ipdb)->pos),pdb+is-1); found=1; } // this contains the corresponding positions in the pdb
}
if (found==0) Error("Cannot find atom in SetRotamersSimilarToPDB");
}
// find the best rotamer
rmsd2min=99999.;
irotmin=0;
for(ir=0;ir< (((p+ic)->back)+i)->nrot;ir++)
{
(((p+ic)->back)+i)->irot = ir;
AddSidechain(p,i,i,ic);
for(is=1;is<(((p+ic)->back)+i)->nside; is++) CopyVector( &((((((p+ic)->back)+i)->side)+is)->pos),cpol+is-1 );
x = DumbRMSD2( pdb, cpol, (((p+ic)->back)+i)->nside-1 );
if (x<rmsd2min) { rmsd2min=x; irotmin=ir; };
}
// set rotamer and cartesian coordinates
(((p+ic)->back)+i)->irot = irotmin;
fprintf(stderr,"%d %d irotmin=%d/%d\n",ic,i,irotmin,(((p+ic)->back)+i)->nrot);
AddSidechain(p,i,i,ic);
}
}
void SubtractVector(float* vector1, float* vector2, float* result) {
float temp[3];
CopyVector(vector2, temp);
for(int i=0; i<3; i++) {
temp[i] *= -1.0;
}
AddVector(vector1, temp, result);
}
double DexAnalogMixin::FilterManipulandumRotations( Vector3 rotations ) {
// Combine the new rotations sample with previous filtered value (recursive filtering).
ScaleVector( filteredManipulandumRotations, filteredManipulandumRotations, filterConstant );
AddVectors( filteredManipulandumRotations, filteredManipulandumRotations, rotations );
ScaleVector( filteredManipulandumRotations, filteredManipulandumRotations, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( rotations, filteredManipulandumRotations );
return( VectorNorm( rotations ) );
}
double DexAnalogMixin::FilterCoP( int which_ati, Vector3 center_of_pressure ) {
// Combine the new force sample with previous filtered value (recursive filtering).
ScaleVector( filteredCoP[which_ati], filteredCoP[which_ati], filterConstant );
AddVectors( filteredCoP[which_ati], filteredCoP[which_ati], center_of_pressure );
ScaleVector( filteredCoP[which_ati], filteredCoP[which_ati], 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( center_of_pressure, filteredCoP[which_ati] );
return( VectorNorm( filteredCoP[which_ati] ) );
}
double DexAnalogMixin::FilterLoadForce( Vector3 load_force ) {
// Combine the new force sample with previous filtered value (recursive filtering).
ScaleVector( filteredLoadForce, filteredLoadForce, filterConstant );
AddVectors( filteredLoadForce, filteredLoadForce, load_force );
ScaleVector( filteredLoadForce, filteredLoadForce, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( load_force, filteredLoadForce );
return( VectorNorm( filteredLoadForce ) );
}
SpringHook::SpringHook(dBodyID _body,const Vector3& _worldpt,const Vector3& _target,Real _k)
:body(_body),target(_target),k(_k)
{
Matrix3 R;
Vector3 t;
CopyVector(t,dBodyGetPosition(body));
CopyMatrix(R,dBodyGetRotation(body));
R.mulTranspose(_worldpt-t,localpt);
}
void interpolate(const dContactGeom& a,const dContactGeom& b,Real u,dContactGeom& x)
{
for(int i=0;i<3;i++) {
x.pos[i] = (1-u)*a.pos[i]+u*b.pos[i];
}
Vector3 an,bn;
CopyVector(an,a.normal);
CopyVector(bn,b.normal);
Vector3 n = (1-u)*an+u*bn;
n.inplaceNormalize();
CopyVector(x.normal,n);
x.depth = Max(a.depth,b.depth);
x.g1 = a.g1;
x.g2 = a.g2;
Assert(x.g1 == b.g1);
Assert(x.g2 == b.g2);
}
double DexAnalogMixin::FilterAcceleration( Vector3 acceleration ) {
// Combine the new position sample with previous filtered value (recursive filtering).
ScaleVector( filteredAcceleration, filteredAcceleration, filterConstant );
AddVectors( filteredAcceleration, filteredAcceleration, acceleration );
ScaleVector( filteredAcceleration, filteredAcceleration, 1.0 / (1.0 + filterConstant ));
// Return the filtered value in place.
CopyVector( acceleration, filteredAcceleration );
return( VectorNorm( acceleration ) );
}
// Constructors
MaterialInterfaceNode::MaterialInterfaceNode(NodalPoint *nd,int vf,int i,int j,
Vector *fImp,double iEnergy,MaterialInterfaceNode *last)
{
theNode=nd; // the node with an interface
vfld = vf; // the crack velocity field
mati = i; // the material velocity field
matipaired = j; // the other material (or -1 if more than one other material)
prevBC = last;
CopyVector(&traction, fImp);
energy = iEnergy;
}
void DexMouseTracker::GetUnitTransform( int unit, Vector3 &offset, Matrix3x3 &rotation ) {
// Simulate a placement error by swapping the units;
if ( !IsDlgButtonChecked( dlg, IDC_CODA_POSITIONED ) ) {
unit = 1 - unit;
}
// Return constant values for the Coda unit placement.
// The real tracker will compute these from the Coda transformations.
if ( TRACKER_ALIGNED_SUPINE == SendDlgItemMessage( dlg, IDC_ALIGNMENT, CB_GETCURSEL, 0, 0 ) ) {
CopyVector( offset, SimulatedSupineCodaOffset[unit] );
CopyMatrix( rotation, SimulatedSupineCodaRotation[unit] );
}
else {
CopyVector( offset, SimulatedUprightCodaOffset[unit] );
CopyMatrix( rotation, SimulatedUprightCodaRotation[unit] );
}
}
// these are a triangle face ccw when looking from outside the polyhedron
PolyTriangle::PolyTriangle(Vector vp0,Vector vp1,Vector vp2)
{
v0=vp0;
v1=vp1;
v2=vp2;
// normal is (v1-v0) X (v2-v0)
Vector a,b;
SubVector(CopyVector(&a,&v1),&v0);
SubVector(CopyVector(&b,&v2),&v0);
CrossProduct(&n,&a,&b);
if(!DbleEqual(n.x,0))
{ m11 = b.z/n.x;
m12 = -b.y/n.x;
m21 = -a.z/n.x;
m22 = a.y/n.x;
style = USE_NX;
}
else if(!DbleEqual(n.z,0))
{ m11 = b.y/n.z;
m12 = -b.x/n.z;
m21 = -a.y/n.z;
m22 = a.x/n.z;
style = USE_NZ;
}
else
{ m11 = -b.z/n.y;
m12 = b.x/n.y;
m21 = a.z/n.y;
m22 = -a.x/n.y;
style = USE_NY;
}
fmin.x = fmin(fmin(v0.x,v1.x),v2.x);
fmin.y = fmin(fmin(v0.y,v1.y),v2.y);
fmin.z = fmin(fmin(v0.z,v1.z),v2.z);
fmax.x = fmax(fmax(v0.x,v1.x),v2.x);
fmax.y = fmax(fmax(v0.y,v1.y),v2.y);
fmax.z = fmax(fmax(v0.z,v1.z),v2.z);
}
int AseFile::ReadLightObjectNodeTM(z_Light &light)
{
char temp[255];
do
{
if( !fgets(temp, 255, file) )return 0;
if( EqualString( temp, "*TM_POS ") ) { CopyVector( temp, light.lpos); continue; }
else ReadUnknown( temp);
}while(!FindBracketClose( temp)); // while temp not contain '}'
return 1;
}
void SpringHook::Step(Real dt)
{
Matrix3 R;
Vector3 t;
Vector3 wp,f;
CopyVector(t,dBodyGetPosition(body));
CopyMatrix(R,dBodyGetRotation(body));
wp = R*localpt+t;
f = k*(target-wp);
//cout<<"Target "<<target<<", world point "<<wp<<", force "<<f<<endl;
dBodyAddForceAtPos(body,f.x,f.y,f.z,wp.x,wp.y,wp.z);
}
OpenGLObject::OpenGLObject( void ) {
texture = NULL;
umag = 1.0;
vmag = 1.0;
enabled = true;
list = -1;
CopyVector( position, zeroVector );
CopyMatrix( orientation, identityMatrix );
CopyVector( offset, zeroVector );
CopyMatrix( attitude, identityMatrix );
init_gl_rotation( gl_attitude );
init_gl_rotation( gl_orientation );
init_gl_displacement( gl_position );
init_gl_displacement( gl_offset );
SetColor( 0.0, 0.0, 0.0, USE_PARENT_COLOR );
}
void ODESimulator::Step(Real dt)
{
Assert(timestep == 0);
//Timer timer;
//double collisionTime,stepTime,updateTime;
gContacts.clear();
timestep=dt;
DetectCollisions();
//printf(" %d contacts detected\n",gContacts.size());
//collisionTime = timer.ElapsedTime();
//timer.Reset();
StepDynamics(dt);
//stepTime = timer.ElapsedTime();
//timer.Reset();
for(map<pair<ODEObjectID,ODEObjectID>,ODEContactList>::iterator i=contactList.begin();i!=contactList.end();i++) {
ODEContactList& cl=i->second;
cl.forces.clear();
for(size_t j=0;j<cl.feedbackIndices.size();j++) {
int k=cl.feedbackIndices[j];
Assert(k >= 0 && k < (int)gContacts.size());
list<ODEContactResult>::iterator cres=gContacts.begin();
advance(cres,k);
Vector3 temp;
for(size_t i=0;i<cres->feedback.size();i++) {
CopyVector(temp,cres->feedback[i].f1);
cl.forces.push_back(temp);
/*
if(!cl.points.back().isValidForce(-temp)) {
printf("ODESimulator: Warning, solved contact force %d %d is invalid\n",k,i);
cout<<" Force: "<<-temp<<", normal "<<cl.points.back().n<<" kFriction "<<cl.points.back().kFriction<<endl;
}
*/
}
}
if(!cl.feedbackIndices.empty())
Assert(cl.points.size() == cl.forces.size());
}
timestep = 0;
//KH: commented this out so GetContacts() would work for ContactSensor simulation. Be careful about loading state
//gContacts.clear();
//updateTime = timer.ElapsedTime();
//printf("ODE simulation step: %g collision, %g step, %g update\n",collisionTime,stepTime,updateTime);
}
/* CloneSVector: return a clone of given matrix */
SVector CloneSVector(MemHeap *hmem, SVector s, Boolean sharing)
{
SVector t; /* the target */
if (s==NULL) return NULL;
if (GetUse(s)>0 && sharing) {
IncUse(s);
return s;
}
t = CreateSVector(hmem,VectorSize(s));
CopyVector(s,t);
return t;
}
