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; }