cpSpace* cpSpaceSerializer::load(cpSpace *space, const char* filename)
{
if (!_doc.LoadFile(filename))
return space;
//Grab our space
TiXmlElement *root = _doc.FirstChildElement("space");
if (!root)
return space;
_space = space;
//Initialize
_bodyMap.clear();
_shapeMap.clear();
//A body id of zero is the space's static body
_bodyMap[0] = space->staticBody;
space->iterations = createValue<int>("iterations", root);
space->gravity = createPoint("gravity", root);
space->damping = createValue<cpFloat>("damping", root);
TiXmlElement *child = root->FirstChildElement("shape");
//Read Shapes
while (child)
{
//attempt a shape
cpShape *shape = createShape(child);
if (shape)
{
//This should not happen like this, need to reflect reality -rkb
if (shape->body->m != INFINITY && !cpSpaceContainsBody(space, shape->body))
cpSpaceAddBody(space, shape->body);
cpSpaceAddShape(space, shape);
}
//Next!
child = child->NextSiblingElement("shape");
}
//Read Constraints
child = root->FirstChildElement("constraint");
while (child)
{
//else attempt a constraint
cpConstraint *constraint = createConstraint(child);
if (constraint)
cpSpaceAddConstraint(space, constraint);
child = child->NextSiblingElement("constraint");
}
return space;
}
void HapticDevice::feedback(btDynamicsWorld &dynamic)
{
for(unsigned int i=0;i<m_nbDevices;i++)
{
bool ground_collide = false;
// free move
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 )// (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_2))
{
m_hss[i].m_free.m_done = true;
//m_hss[i].m_free.m_force = hduVector3Dd(0,0,0);
}
if(m_constraints[i] != NULL)
{
btRigidBody * myBody = &m_constraints[i]->getRigidBodyB();
btTransform myTrans = myBody->getWorldTransform();
m_effRenderPos = myTrans.getOrigin();
//Check collision
if(m_constraints[i]->getUserConstraintPtr() != NULL)
{
//std::cout<< " se cas la " <<std::endl;
m_hss[i].m_free.m_nbCollision = 1;
//if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_1) == 0)
btCollisionObject * object = static_cast<btCollisionObject *>(m_constraints[i]->getUserConstraintPtr());
if(object->getInternalType()== btCollisionObject::CO_RIGID_BODY)
{
btRigidBody * collideBody = static_cast<btRigidBody *>(object);
// collide with ground
if(collideBody == m_ground)
{
m_hss[i].m_free.m_nbCollision = 2;
ground_collide = true;
}
// collide with other object
if(collideBody->getInvMass()!=0 && collideBody != m_ground)
{
if(m_itsConstraints[i] == NULL )
{
// catch it if colide with it
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) == 0 )
{
//create constraint
btTransform bodyTrans = collideBody->getWorldTransform();
m_itsConstraints[i] = createConstraint(*myBody,*collideBody);
dynamic.addConstraint(m_itsConstraints[i],true);
m_newConstraint(m_ptr,collideBody,i);
m_caught = collideBody;
m_coll = true;
m_devine = false;
showTarget(collideBody);
deactivateMove();
}
}else
// realise it when button 1 pressed
if(m_freeT || (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0)
{
//remove constraint
dynamic.removeConstraint(m_itsConstraints[i]);
delete m_itsConstraints[i];
m_itsConstraints[i]=NULL;
m_deleteConstraint(m_ptr,collideBody,i);
//m_hss[i].setThrown(NULL);
m_hss[i].m_free.m_done = true;
showTarget(collideBody);
m_variator = 0;
m_coll = false;
deactivateMove();
}
}
}
}
else
{
m_hss[i].m_free.m_nbCollision = 0;
//if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_1) == 0)
if(m_freeT || (m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_1) != 0)
{
if(m_itsConstraints[i] != NULL )
{
//remove constraint
m_deleteConstraint(m_ptr,&m_itsConstraints[i]->getRigidBodyB(),i);
dynamic.removeConstraint(m_itsConstraints[i]);
delete m_itsConstraints[i];
m_itsConstraints[i]=NULL;
m_hss[i].m_free.m_done = true;
if(m_caught != NULL)
showTarget(m_caught);
m_coll = false;
deactivateMove();
}
}
}
if(m_itsConstraints[i]!=NULL )
m_hss[i].m_free.m_nbCollision = 1;
else
// launch an other target
if((m_hss[i].m_free.m_buttons & HD_DEVICE_BUTTON_2) != 0 && (m_oldButtons[i] & HD_DEVICE_BUTTON_2) == 0)
{
//m_canLaunch = true;
}
m_oldButtons[i]=m_hss[i].m_free.m_buttons;
//put back cursor world position into device referencial <<<<< ------------------------------
btTransform offset(btMatrix3x3::getIdentity(),btVector3(0,0,-0.5));
m_effectors[i].setOrigin(myTrans.getOrigin());
m_effectors[i].mult(m_effectors[i],offset);
btVector3 pos = m_cameraViews[i]->inverse()(myTrans.getOrigin());
pos*=SCALE_WORLD_TO_DEVICE;
m_hss[i].m_free.m_realPosition.set(pos.getX(),pos.getY(),pos.getZ()+OFFSET_TO_CAMERA);
}
btTransform camInv = m_cameraViews[i]->inverse();
// compute feed back for ground
if(ground_collide)
{
m_hss[i].m_free.m_force = groundForce(true,&(m_hss[i].m_free.m_position),&camInv);
}else
m_hss[i].m_free.m_force = hduVector3Dd(0,0,0);
// detecte the direction
HDdouble deplacement = betweenTwoPoints(m_hss[i].m_free.m_atThrowPos,m_hss[i].m_free.m_position);
HDdouble distanceMax = Distance_max;
hduVector3Dd move = m_hss[i].m_free.m_oldPosition - m_hss[i].m_free.m_position;
HDdouble selectedDistance = 0;
if( m_devine && m_thrownRigids != NULL && m_thrownRigids->size()>0 && deplacement > distanceMax && m_sible == 0 /* && !m_targetChoosen */){
HDdouble distance = Quick_Distance_max;
//unsigned int targ = 0;
for(unsigned int j = 0; j<m_thrownRigids->size(); j++){
HDdouble d = distanceToTrajectory(m_hss[i].m_free.m_position,j,&camInv);
if((d)<distance){
if(m_Thrown != (*m_thrownRigids)[j]){
m_hss[i].m_free.m_currentThrown = (*m_thrownRigids)[j];
distance = d;
m_Thrown = (*m_thrownRigids)[j];
m_impactPos = m_possibleImpact[j];
m_targetChoosen = true;
if(m_Feedback){
activateMove();
}
selectedDistance = d;
m_index = j;
m_sible = Nbr_frame_wait;
}
}
}
if(m_targetChoosen){
//showTarget(m_Thrown);
addPrevious(m_Thrown);
}
else
m_sible = 0;
//m_time = Time;
}
if(m_canLaunch){
//m_time = Time;
m_hss[i].m_free.m_done = true;
m_variator = 0.001;
m_sible = 0;
m_devine = true;
deactivateMove();
cleanHistory();
m_hss[i].m_free.m_atThrowPos = m_hss[i].m_free.m_position;
}
m_hss[i].m_free.m_oldPosition = m_hss[i].m_free.m_position ;
if(m_hss[i].m_free.m_currentThrown != NULL && m_posSet && m_Feedback ){
hduVector3Dd impact = invertTransform(m_impactPos, &camInv);
hduVector3Dd pos(m_hss[i].m_free.m_position);
btVector3 balltest = ((*m_thrownRigids)[0])->getWorldTransform().getOrigin();
hduVector3Dd test = invertTransform(&balltest, &camInv);
m_velocity = m_hss[i].m_free.m_velocity.magnitude();
if(pos[2]-5 >= test[2]){
if(!m_hss[i].m_free.m_done){
//hduVector3Dd helpForce = ForceToImpact(&pos,&impact);
hduVector3Dd helpForce = magneticForce(&pos, &impact, &camInv);
//hduVector3Dd helpForce = atomeForce(&pos, &impact, &camInv);
if(!helpForce.isZero(EPSILON)){
hduVector3Dd force = 1.0 * helpForce;
m_hss[i].m_free.m_force = force;
m_Force = force;
}else
{
//m_hss[i].m_free.m_force = m_Force;
}
}
}
}
hdScheduleSynchronous(sScheduleIn, &m_hss, HD_DEFAULT_SCHEDULER_PRIORITY);
m_selectedDistance = selectedDistance;
if(m_sible > 0)
m_sible--;
}
}
//---------------------------------------------------------------------------
CSPProblem *RandomCSPFactory::create( int n, int m, double p1, double p2, bool forceSatisfiable )
{
int i, j;
CSPInterpretation interpretation;
if ( forceSatisfiable )
{
interpretation.setNumVariables( n );
for ( i = 0; i < n; i++ )
{
interpretation.setVariableValue( i, (int)Random::s_generateUniformVariable( 0, m ) );
}
}
// Create the problem object
CSPProblem *problem = CSPProblem::create();
problem->setNumVars( n );
BasicCSPDomainList *domainList = BasicCSPDomainList::create();
domainList->setNumDomains( n );
CSPVariableDomain *domain = CSPVariableDomain::create( m );
for ( i = 0; i < m; i++ )
{
domain->addValue( i, BasicCSPVariableValue::create( i ) );
}
for ( j = 0; j < n; j++ )
{
domainList->setDomain( j, domain->clone() );
}
domain->release();
domain = NULL;
problem->setDomainList( domainList );
BasicCSPConstraintList *constraintList = BasicCSPConstraintList::create();
for ( i = 0; i < n; i++ )
{
for ( j = i + 1; j < n; j++ )
{
double random = Random::s_generateUnitUniformVariable();
if ( random < p1 )
{
constraintList->addConstraint( createConstraint( problem, i, j, m, p2, interpretation, forceSatisfiable ) );
}
}
}
problem->setConstraintList( constraintList );
// test the problem against the interpretation
if ( forceSatisfiable )
{
if ( problem->isSatisfied( interpretation ) == false )
{
return NULL;
}
}
return problem;
}
void generateConstraints(vector<Node*>& nodes, vector<Edge*>& edges,vector<SimpleConstraint*>& cs,Dim dim) {
unsigned nevents=2*nodes.size()+2*edges.size();
events=new Event*[nevents];
unsigned ctr=0;
if(dim==HORIZONTAL) {
//cout << "Scanning top to bottom..." << endl;
for(unsigned i=0;i<nodes.size();i++) {
Node *v=nodes[i];
v->scanpos=v->x;
events[ctr++]=new Event(Open,v,v->ymin+0.01);
events[ctr++]=new Event(Close,v,v->ymax-0.01);
}
for(unsigned i=0;i<edges.size();i++) {
Edge *e=edges[i];
events[ctr++]=new Event(Open,e,e->ymin-1);
events[ctr++]=new Event(Close,e,e->ymax+1);
}
} else {
//cout << "Scanning left to right..." << endl;
for(unsigned i=0;i<nodes.size();i++) {
Node *v=nodes[i];
v->scanpos=v->y;
events[ctr++]=new Event(Open,v,v->xmin+0.01);
events[ctr++]=new Event(Close,v,v->xmax-0.01);
}
for(unsigned i=0;i<edges.size();i++) {
Edge *e=edges[i];
events[ctr++]=new Event(Open,e,e->xmin-1);
events[ctr++]=new Event(Close,e,e->xmax+1);
}
}
qsort((Event*)events, (size_t)nevents, sizeof(Event*), compare_events );
NodeSet openNodes;
vector<Edge*> openEdges;
for(unsigned i=0;i<nevents;i++) {
Event *e=events[i];
Node *v=e->v;
if(v!=NULL) {
v->open = true;
//printf("NEvent@%f,nid=%d,(%f,%f),w=%f,h=%f,openn=%d,opene=%d\n",e->pos,v->id,v->x,v->y,v->width,v->height,(int)openNodes.size(),(int)openEdges.size());
Node *l=NULL, *r=NULL;
if(!openNodes.empty()) {
// it points to the first node to the right of v
NodeSet::iterator it=openNodes.lower_bound(v);
// step left to find the first node to the left of v
if(it--!=openNodes.begin()) {
l=*it;
//printf("l=%d\n",l->id);
}
it=openNodes.upper_bound(v);
if(it!=openNodes.end()) {
r=*it;
}
}
vector<Node*> L;
sortNeighbours(v,l,r,e->pos,openEdges,L,nodes,dim);
//printf("L=[");
for(unsigned i=0;i<L.size();i++) {
//printf("%d ",L[i]->id);
}
//printf("]\n");
// Case A: create constraints between adjacent edges skipping edges joined
// to l,v or r.
Node* lastNode=NULL;
for(vector<Node*>::iterator i=L.begin();i!=L.end();i++) {
if((*i)->dummy) {
// node is on an edge
Edge *edge=(*i)->edge;
if(!edge->isEnd(v->id)
&&((l!=NULL&&!edge->isEnd(l->id))||l==NULL)
&&((r!=NULL&&!edge->isEnd(r->id))||r==NULL)) {
if(lastNode!=NULL) {
//printf(" Rule A: Constraint: v%d +g <= v%d\n",lastNode->id,(*i)->id);
cs.push_back(createConstraint(lastNode,*i,dim));
}
lastNode=*i;
}
} else {
// is an actual node
lastNode=NULL;
}
}
// Case B: create constraints for all the edges connected to the right of
// their own end, also in the scan line
vector<Node*> skipList;
lastNode=NULL;
for(vector<Node*>::iterator i=L.begin();i!=L.end();i++) {
if((*i)->dummy) {
// node is on an edge
if(lastNode!=NULL) {
if((*i)->edge->isEnd(lastNode->id)) {
skipList.push_back(*i);
} else {
for(vector<Node*>::iterator j=skipList.begin();
j!=skipList.end();j++) {
//printf(" Rule B: Constraint: v%d +g <= v%d\n",(*j)->id,(*i)->id);
cs.push_back(createConstraint(*j,*i,dim));
}
skipList.clear();
}
}
} else {
// is an actual node
skipList.clear();
skipList.push_back(*i);
lastNode=*i;
}
}
skipList.clear();
// Case C: reverse of B
lastNode=NULL;
for(vector<Node*>::reverse_iterator i=L.rbegin();i!=L.rend();i++) {
if((*i)->dummy) {
// node is on an edge
if(lastNode!=NULL) {
if((*i)->edge->isEnd(lastNode->id)) {
skipList.push_back(*i);
} else {
for(vector<Node*>::iterator j=skipList.begin();
j!=skipList.end();j++) {
//printf(" Rule C: Constraint: v%d +g <= v%d\n",(*i)->id,(*j)->id);
cs.push_back(createConstraint(*i,*j,dim));
}
skipList.clear();
}
}
} else {
// is an actual node
skipList.clear();
skipList.push_back(*i);
lastNode=*i;
}
}
if(e->type==Close) {
if(l!=NULL) cs.push_back(createConstraint(l,v,dim));
if(r!=NULL) cs.push_back(createConstraint(v,r,dim));
}
}
if(e->type==Open) {
if(v!=NULL) {
openNodes.insert(v);
} else {
//printf("EdgeOpen@%f,eid=%d,(u,v)=(%d,%d)\n", e->pos,e->e->id,e->e->startNode,e->e->endNode);
e->e->openInd=openEdges.size();
openEdges.push_back(e->e);
}
} else {
// Close
if(v!=NULL) {
openNodes.erase(v);
v->open=false;
} else {
//printf("EdgeClose@%f,eid=%d,(u,v)=(%d,%d)\n", e->pos,e->e->id,e->e->startNode,e->e->endNode);
unsigned i=e->e->openInd;
openEdges[i]=openEdges[openEdges.size()-1];
openEdges[i]->openInd=i;
openEdges.resize(openEdges.size()-1);
}
}
delete e;
}
delete [] events;
}