void HelloWorld::moveYCallback(Ref * pSender) { auto animation = Animation::createWithSpriteFrames(attack, 0.1f); auto animate = Animate::create(animation); auto idleAnimation = Animation::createWithSpriteFrames(idle, 0.1f); auto idleAnimate = Animate::create(idleAnimation); auto seq = Sequence::create(animate, idleAnimate, CCCallFunc::create(this, callfunc_selector(HelloWorld::enCallback)), NULL); if (en == false) { en = true; player->runAction(seq); } auto fac = Factory::getInstance(); Rect playerRect = player->getBoundingBox(); Rect attackRect=Rect(playerRect.getMinX()-40,playerRect.getMinY(), playerRect.getMaxX()-playerRect.getMinX()+80, playerRect.getMaxY() - playerRect.getMinY()); Sprite* collision = fac->collider(attackRect); if (collision != NULL) { fac->removeMonster(collision); hp += 20; if (hp > 100) hp = 100; CCProgressTo* ac1 = CCProgressTo::create(2.0f, hp); pT->runAction(ac1); ++sc; auto temp = CCString::createWithFormat("%d", sc); score->setString(temp->getCString()); UserDefault::getInstance()->setIntegerForKey("score", sc); } }
void b2CollideEdgeAndPolygon( b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA, const b2PolygonShape* polygonB, const b2Transform& xfB) { b2EPCollider collider(edgeA, xfA, polygonB, xfB); collider.Collide(manifold); }
b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy( const b3Vector3& aabbMin, const b3Vector3& aabbMax, int objectId, void* userPtr, short int collisionFilterGroup, short int collisionFilterMask) { b3DbvtProxy* mem = &m_proxies[objectId]; b3DbvtProxy* proxy=new(mem) b3DbvtProxy( aabbMin,aabbMax,userPtr, collisionFilterGroup, collisionFilterMask); b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax); //bproxy->aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax); proxy->stage = m_stageCurrent; proxy->m_uniqueId = objectId; proxy->leaf = m_sets[0].insert(aabb,proxy); b3ListAppend(proxy,m_stageRoots[m_stageCurrent]); if(!m_deferedcollide) { b3DbvtTreeCollider collider(this); collider.proxy=proxy; m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); } return(proxy); }
btBroadphaseProxy* btDbvtBroadphase::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int /*shapeType*/, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/) { btDbvtProxy* proxy = new (btAlignedAlloc(sizeof(btDbvtProxy), 16)) btDbvtProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask); btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin, aabbMax); //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); proxy->stage = m_stageCurrent; proxy->m_uniqueId = ++m_gid; proxy->leaf = m_sets[0].insert(aabb, proxy); listappend(proxy, m_stageRoots[m_stageCurrent]); if (!m_deferedcollide) { btDbvtTreeCollider collider(this); collider.proxy = proxy; m_sets[0].collideTV(m_sets[0].m_root, aabb, collider); m_sets[1].collideTV(m_sets[1].m_root, aabb, collider); } return (proxy); }
void undrawpiece(void) { Image *mask = nil; if(collider(pos, br.max)) mask = bbmask; draw(screen, rectaddpt(br, pos), display->white, mask, bb->r.min); }
void playback_exact_CSpace_R3() { C2A_Model* P = NULL; C2A_Model* Q = NULL; readObjFile(P, "../data/models/CupSpoon/Cup.obj"); readObjFile(Q, "../data/models/CupSpoon/Spoon.obj"); P->ComputeRadius(); Q->ComputeRadius(); Collider3D collider(P, Q); C2A_Model* CSpace; readObjFile(CSpace, "../data/cupspoon.obj"); std::vector<ContactSpaceSampleData> contactspace_samples; std::ifstream in("space_test_3d.txt"); asciiReader(in, contactspace_samples); std::ofstream timing_file("timing_exact_R3.txt"); std::ofstream PD_file("PD_exact_R3.txt"); for(std::size_t i = 0; i < contactspace_samples.size(); ++i) { std::cout << i << std::endl; DataVector q_col(6); DataVector q(3); for(std::size_t j = 0; j < 3; ++j) q[j] = contactspace_samples[i].v[j]; for(std::size_t j = 0; j < 6; ++j) q_col[j] = contactspace_samples[i].v[j]; boost::timer t; //aTimer.Reset(); aTimer.Start(); std::pair<DataVector, double> pd_result; if(!collider.isCollide(q_col)) { pd_result.second = 0; } else { pd_result = Minkowski_Cspace_3D::Exact_PD_R3(q, CSpace); } aTimer.Stop(); PD_file << pd_result.second << " "; //timing_file << t.elapsed() << " "; timing_file << aTimer.GetTime() * 1000 << " "; timing_file.flush(); PD_file.flush(); } }
inline static Ptr create(ColliderDataPtr data) { Ptr collider(new Collider(data)); collider->initialize(); return collider; }
void PhysicsSystem::Update( f32 dt ) { //add gravity for (RigidBody* rb : rigidBodies) { rb->ApplyForce(rb->m * gravity); //rb->ApplyImpulse({ 0.f, -9.81f },{ 0.f, -9.81f }); //rb->Update(dt); //rb->forces = {0.0,0.0}; } std::vector<CollisionInfo> collisions; u32 count = rigidBodies.size(); //generate contact infos for (u32 i = 0; i< count -1; i++) { for (u32 j = i+1; j < count; j++) { CollisionInfo info; RigidBody * rb1 = rigidBodies[i]; RigidBody * rb2 = rigidBodies[j]; if (collider(rb1,rb2,info)) { collisions.push_back(info); //solveur } } } //Integrate Forces for (RigidBody* rb : rigidBodies) { rb->IntegrateForces(dt); // linearVelocity+=a*dt; // angularVelocity += theta*dt; } // solve contacts for (auto collision : collisions) { collision.Solve(); collision.correction(); } //integrate velocities for(auto &rb : rigidBodies ) { rb->IntegrateVelocities(dt); //position+=v*dt; rotation+=w*dt; } //clear forces for( auto & rb : rigidBodies ) { rb->forces = {0.0,0.0}; rb->torque = 0.0; } }
int movepiece(void) { Image *mask = nil; if(collide(Pt(pos.x, pos.y+pcsz), piece)) return 0; if(collider(pos, br2.max)) mask = bb2mask; draw(screen, rectaddpt(br2, pos), bb2, mask, bb2->r.min); pos.y += DY; flushimage(display, 1); return 1; }
void SimpleShear::createActors(shared_ptr<Scene>& scene) { shared_ptr<IGeomDispatcher> interactionGeometryDispatcher(new IGeomDispatcher); interactionGeometryDispatcher->add(new Ig2_Sphere_Sphere_ScGeom6D); interactionGeometryDispatcher->add(new Ig2_Box_Sphere_ScGeom6D); shared_ptr<IPhysDispatcher> interactionPhysicsDispatcher(new IPhysDispatcher); shared_ptr<IPhysFunctor> CL1Rel(new Ip2_2xNormalInelasticMat_NormalInelasticityPhys); interactionPhysicsDispatcher->add(CL1Rel); shared_ptr<InsertionSortCollider> collider(new InsertionSortCollider); collider->boundDispatcher->add(new Bo1_Sphere_Aabb); collider->boundDispatcher->add(new Bo1_Box_Aabb); shared_ptr<GravityEngine> gravityCondition(new GravityEngine); gravityCondition->gravity = gravity; shared_ptr<GlobalStiffnessTimeStepper> globalStiffnessTimeStepper(new GlobalStiffnessTimeStepper); globalStiffnessTimeStepper->timeStepUpdateInterval = timeStepUpdateInterval; globalStiffnessTimeStepper->defaultDt=3e-6; shared_ptr<KinemCTDEngine> kinemEngine (new KinemCTDEngine); kinemEngine->compSpeed = 10.0; kinemEngine->targetSigma=2000.0; shared_ptr<InteractionLoop> ids(new InteractionLoop); ids->geomDispatcher=interactionGeometryDispatcher; ids->physDispatcher=interactionPhysicsDispatcher; ids->lawDispatcher=shared_ptr<LawDispatcher>(new LawDispatcher); shared_ptr<Law2_ScGeom6D_NormalInelasticityPhys_NormalInelasticity> ldc(new Law2_ScGeom6D_NormalInelasticityPhys_NormalInelasticity); ids->lawDispatcher->add(ldc); scene->engines.clear(); scene->engines.push_back(shared_ptr<Engine>(new ForceResetter)); scene->engines.push_back(globalStiffnessTimeStepper); scene->engines.push_back(collider); scene->engines.push_back(ids); if(gravApplied) scene->engines.push_back(gravityCondition); scene->engines.push_back(shared_ptr<Engine> (new NewtonIntegrator)); scene->engines.push_back(kinemEngine); }
bool Player::checkChunkIntersection(Manifold& manifold) { // Retrieve blocks for current chunk if (m_chunkValid) { const std::vector<glm::vec3>& blockPositions = m_currentChunk.getBlockPositions(); // Create player collider glm::vec3 playerMinPoint(m_currentPosition.x - 0.5f, m_currentPosition.y - 1.0f, m_currentPosition.z - 0.5f); glm::vec3 playerMaxPoint(m_currentPosition.x + 0.5f, m_currentPosition.y + 1.0f, m_currentPosition.z + 0.5f); AABBCollider playerCollider(playerMinPoint, playerMaxPoint); for (const glm::vec3& block : blockPositions) { // Create AABB glm::vec3 minPoint(block.x - 0.5f, block.y - 0.5f, block.z - 0.5f); glm::vec3 maxPoint(block.x + 0.5f, block.y + 0.5f, block.z + 0.5f); AABBCollider collider(minPoint, maxPoint); // Check intersection if (AABBCollider::checkCollision(playerCollider, collider, manifold)) { //std::cout << "PIGNGNOFNOSFOSFNFON" << std::endl; // ########################################################################################### //glm::vec3 directionOfMovement(m_currentPosition - m_lastPosition); //std::cout << "(" << directionOfMovement.x << ", " << directionOfMovement.y << ", " << directionOfMovement.z << ")" << std::endl; // //glm::vec3 displacement-directionO; // //// If im moving in the positive x //if (directionOfMovement.x > 0) { // //} //else if (directionOfMovement.x < 0) { // //} //else { // displacement.x = 0; //} resolveChunkIntersection(manifold, collider); return true; } } } return false; }
void HelloWorld::hitByMonster(float dt) { auto fac = Factory::getInstance(); Sprite* collision = fac->collider(player->getBoundingBox()); if (collision != NULL) { fac->removeMonster(collision); hp -= 20; if (hp <= 0) { CCProgressTo* ac1 = CCProgressTo::create(2.0f, 0); auto seq = Sequence::create(ac1, CCCallFunc::create(this, callfunc_selector(HelloWorld::endCallback)), NULL); pT->runAction(seq); } else { CCProgressTo* ac1 = CCProgressTo::create(2.0f, hp); pT->runAction(ac1); } } }
void btDbvtBroadphase::setAabbForceUpdate(btBroadphaseProxy* absproxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/) { btDbvtProxy* proxy = (btDbvtProxy*)absproxy; ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb = btDbvtVolume::FromMM(aabbMin, aabbMax); bool docollide = false; if (proxy->stage == STAGECOUNT) { /* fixed -> dynamic set */ m_sets[1].remove(proxy->leaf); proxy->leaf = m_sets[0].insert(aabb, proxy); docollide = true; } else { /* dynamic set */ ++m_updates_call; /* Teleporting */ m_sets[0].update(proxy->leaf, aabb); ++m_updates_done; docollide = true; } listremove(proxy, m_stageRoots[proxy->stage]); proxy->m_aabbMin = aabbMin; proxy->m_aabbMax = aabbMax; proxy->stage = m_stageCurrent; listappend(proxy, m_stageRoots[m_stageCurrent]); if (docollide) { m_needcleanup = true; if (!m_deferedcollide) { btDbvtTreeCollider collider(this); m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider); m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider); } } }
void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher) { /*printf("---------------------------------------------------------\n"); printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves); printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs()); { int i; for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++) { printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(), getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid()); } printf("\n"); } */ b3SPC(m_profiling.m_total); /* optimize */ m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); if(m_fixedleft) { const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); m_fixedleft=b3Max<int>(0,m_fixedleft-count); } /* dynamic -> fixed set */ m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; b3DbvtProxy* current=m_stageRoots[m_stageCurrent]; if(current) { b3DbvtTreeCollider collider(this); do { b3DbvtProxy* next=current->links[1]; b3ListRemove(current,m_stageRoots[current->stage]); b3ListAppend(current,m_stageRoots[STAGECOUNT]); #if B3_DBVT_BP_ACCURATESLEEPING m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); collider.proxy=current; b3DynamicBvh::collideTV(m_sets[0].m_root,current->aabb,collider); b3DynamicBvh::collideTV(m_sets[1].m_root,current->aabb,collider); #endif m_sets[0].remove(current->leaf); B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) curAabb=b3DbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); current->leaf = m_sets[1].insert(curAabb,current); current->stage = STAGECOUNT; current = next; } while(current); m_fixedleft=m_sets[1].m_leaves; m_needcleanup=true; } /* collide dynamics */ { b3DbvtTreeCollider collider(this); if(m_deferedcollide) { b3SPC(m_profiling.m_fdcollide); m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); } if(m_deferedcollide) { b3SPC(m_profiling.m_ddcollide); m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); } } /* clean up */ if(m_needcleanup) { b3SPC(m_profiling.m_cleanup); b3BroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); if(pairs.size()>0) { int ni=b3Min(pairs.size(),b3Max<int>(m_newpairs,(pairs.size()*m_cupdates)/100)); for(int i=0;i<ni;++i) { b3BroadphasePair& p=pairs[(m_cid+i)%pairs.size()]; b3DbvtProxy* pa=&m_proxies[p.x]; b3DbvtProxy* pb=&m_proxies[p.y]; if(!b3Intersect(pa->leaf->volume,pb->leaf->volume)) { #if B3_DBVT_BP_SORTPAIRS if(pa->m_uniqueId>pb->m_uniqueId) b3Swap(pa,pb); #endif m_paircache->removeOverlappingPair(pa->getUid(),pb->getUid(),dispatcher); --ni;--i; } } if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; } } ++m_pid; m_newpairs=1; m_needcleanup=false; if(m_updates_call>0) { m_updates_ratio=m_updates_done/(b3Scalar)m_updates_call; } else { m_updates_ratio=0; } m_updates_done/=2; m_updates_call/=2; }
void b3DynamicBvhBroadphase::setAabb( b3BroadphaseProxy* absproxy, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/) { b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy; B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax); #if B3_DBVT_BP_PREVENTFALSEUPDATE if(b3NotEqual(aabb,proxy->leaf->volume)) #endif { bool docollide=false; if(proxy->stage==STAGECOUNT) {/* fixed -> dynamic set */ m_sets[1].remove(proxy->leaf); proxy->leaf=m_sets[0].insert(aabb,proxy); docollide=true; } else {/* dynamic set */ ++m_updates_call; if(b3Intersect(proxy->leaf->volume,aabb)) {/* Moving */ const b3Vector3 delta=aabbMin-proxy->m_aabbMin; b3Vector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); if(delta[0]<0) velocity[0]=-velocity[0]; if(delta[1]<0) velocity[1]=-velocity[1]; if(delta[2]<0) velocity[2]=-velocity[2]; if ( #ifdef B3_DBVT_BP_MARGIN m_sets[0].update(proxy->leaf,aabb,velocity,B3_DBVT_BP_MARGIN) #else m_sets[0].update(proxy->leaf,aabb,velocity) #endif ) { ++m_updates_done; docollide=true; } } else {/* Teleporting */ m_sets[0].update(proxy->leaf,aabb); ++m_updates_done; docollide=true; } } b3ListRemove(proxy,m_stageRoots[proxy->stage]); proxy->m_aabbMin = aabbMin; proxy->m_aabbMax = aabbMax; proxy->stage = m_stageCurrent; b3ListAppend(proxy,m_stageRoots[m_stageCurrent]); if(docollide) { m_needcleanup=true; if(!m_deferedcollide) { b3DbvtTreeCollider collider(this); m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); } } } }
void playback_R3() { C2A_Model* P = NULL; C2A_Model* Q = NULL; readObjFile(P, "../data/models/CupSpoon/Cup.obj"); readObjFile(Q, "../data/models/CupSpoon/Spoon.obj"); P->ComputeRadius(); Q->ComputeRadius(); Collider3D collider(P, Q); std::vector<ContactSpaceSampleData> contactspace_samples; std::ifstream in("space_test_3d.txt"); asciiReader(in, contactspace_samples); ContactSpaceR3 contactspace(P, Q, 0.05 * (P->radius + Q->radius)); std::ofstream scaler_file("scaler_3d_rotation_cupspoon.txt"); scaler_file << contactspace.getScaler() << std::endl; std::vector<ContactSpaceSampleData> train_samples = contactspace.uniform_sample(100000); SVMLearner learner; learner.setDim(contactspace.active_data_dim()); learner.setC(20); learner.setScaler(contactspace.getScaler()); learner.setUseScaler(true); learner.setGamma(50); learner.learn(train_samples, contactspace.active_data_dim()); learner.save("model_R3.txt"); // flann::HierarchicalClusteringIndex<ContactSpaceSE3Euler::DistanceType>* query_index = learner.constructIndexOfSupportVectorsForQuery<ContactSpaceSE3Euler, flann::HierarchicalClusteringIndex, flann::HierarchicalClusteringIndexParams>(); std::vector<ContactSpaceSampleData> support_samples; learner.collectSupportVectors(support_samples); ExtendedModel<ContactSpaceR3, flann::Index> extended_model = constructExtendedModelForModelDecisionBoundary<ContactSpaceR3, SVMLearner, flann::Index, flann::KDTreeIndexParams>(contactspace, learner, support_samples, 0.01, 50); std::ofstream timing_file("timing_APD_R3.txt"); std::ofstream PD_file("PD_APD_R3.txt"); for(std::size_t i = 0; i < contactspace_samples.size(); ++i) { std::cout << i << std::endl; DataVector q_col(6); DataVector q(3); for(std::size_t j = 0; j < 3; ++j) q[j] = contactspace_samples[i].v[j]; for(std::size_t j = 0; j < 6; ++j) q_col[j] = contactspace_samples[i].v[j]; boost::timer t; aTimer.Reset(); aTimer.Start(); if(!collider.isCollide(q_col)) { PD_file << 0 << " "; } else { QueryResult pd_result = PD_query(learner, contactspace, extended_model.index, extended_model.samples, q); PD_file << pd_result.PD << " "; } // timing_file << t.elapsed() << " "; timing_file << aTimer.GetTime() * 1000 << " "; timing_file.flush(); PD_file.flush(); } }
void playback() { std::vector<std::vector<DataVector> > frames; std::string frame_file_name = "../data/models/CupSpoon/CupSpoon_NoJump.ani"; bool use_euler = true; frames = readAnimationFile(frame_file_name, use_euler); C2A_Model* P = NULL; C2A_Model* Q = NULL; readObjFile(P, "../data/models/CupSpoon/Cup.obj"); readObjFile(Q, "../data/models/CupSpoon/Spoon.obj"); P->ComputeRadius(); Q->ComputeRadius(); Collider3D collider(P, Q); { double Ix, Iy, Iz; inertia_weight(Q, Ix, Iy, Iz); distance_weight[0] = 1; distance_weight[1] = 1; distance_weight[2] = 1; std::cout << Ix << " " << Iy << " " << Iz << std::endl; distance_weight[3] = Ix; distance_weight[4] = Iy; distance_weight[5] = Iz; } ContactSpaceSE3Euler contactspace(P, Q, 0.05 * (P->radius + Q->radius)); std::ofstream scaler_file("scaler_3d_rotation_cupspoon.txt"); scaler_file << contactspace.getScaler() << std::endl; std::vector<ContactSpaceSampleData> contactspace_samples = contactspace.uniform_sample(500000); SVMLearner learner; learner.setDim(contactspace.active_data_dim()); learner.setC(20); learner.setScaler(contactspace.getScaler()); learner.setUseScaler(true); learner.setGamma(50); learner.learn(contactspace_samples, contactspace.active_data_dim()); learner.save("model.txt"); // flann::HierarchicalClusteringIndex<ContactSpaceSE3Euler::DistanceType>* query_index = learner.constructIndexOfSupportVectorsForQuery<ContactSpaceSE3Euler, flann::HierarchicalClusteringIndex, flann::HierarchicalClusteringIndexParams>(); std::vector<ContactSpaceSampleData> support_samples; learner.collectSupportVectors(support_samples); ExtendedModel<ContactSpaceSE3Euler, flann::HierarchicalClusteringIndex> extended_model = constructExtendedModelForModelDecisionBoundary<ContactSpaceSE3Euler, SVMLearner, flann::HierarchicalClusteringIndex, flann::HierarchicalClusteringIndexParams>(contactspace, learner, support_samples, 0.01, 50); std::ofstream timing_file("timing_APD.txt"); std::ofstream PD_file("PD_APD.txt"); std::vector<DataVector> result_qs; for(std::size_t i = 0; i < frames.size(); ++i) { DataVector q = relative3D(frames[i][0], frames[i][1]); boost::timer t; if(!collider.isCollide(q)) { result_qs.push_back(frames[i][1]); PD_file << 0 << " "; } else { // QueryResult pd_result = PD_query(learner, contactspace, query_index, q); QueryResult pd_result = PD_query(learner, contactspace, extended_model.index, extended_model.samples, q); DataVector q2 = unrelative3D(frames[i][0], pd_result.v); result_qs.push_back(q2); PD_file << pd_result.PD << " "; } timing_file << t.elapsed() << " "; timing_file.flush(); PD_file.flush(); } std::ofstream new_animation_stream("new_animation_APD.ani"); new_animation_stream << frames.size() << "f" << std::endl; for(std::size_t i = 0; i < frames.size(); ++i) { new_animation_stream << i << "f" << std::endl; double R[3][3]; double T[3]; if(frames[i][0].dim() == 6) { ConfigEuler2RotTran(R, T, frames[i][0]); } else if(frames[i][0].dim() == 7) { ConfigQuat2RotTrans(R, T, frames[i][0]); } for(int j = 0; j < 3; ++j) for(int k = 0; k < 3; ++k) { // new_animation_stream << R[j][k] << " "; new_animation_stream << R[k][j] << " "; } for(int j = 0; j < 3; ++j) new_animation_stream << T[j] << " "; new_animation_stream << std::endl; if(result_qs[i].dim() == 6) { ConfigEuler2RotTran(R, T, result_qs[i]); } else if(result_qs[i].dim() == 7) { ConfigQuat2RotTrans(R, T, result_qs[i]); } for(int j = 0; j < 3; ++j) for(int k = 0; k < 3; ++k) { // new_animation_stream << R[j][k] << " "; new_animation_stream << R[k][j] << " "; } for(int j = 0; j < 3; ++j) new_animation_stream << T[j] << " "; new_animation_stream << std::endl; } }
void playback() { std::ofstream timing_file("timing_APD.txt"); std::ofstream timing_construct_file("timing_construct_APD.txt"); std::string base_name = "../data/models/Box2D/spider_cinfigs/dump_transform"; std::vector<Polygon> poly = readPolyFile("../data/models/Box2D/nazca_spider77.polys", 3); for(std::size_t i = 0; i < 7; ++i) distance_weight[i] = 1; { double Ix, Iy; inertia(poly, Ix, Iy); double rotation_weight = sqrt(Ix * Ix + Iy * Iy); distance_weight[0] = 1; distance_weight[1] = 1; distance_weight[2] = rotation_weight; } ContactSpaceSE2 contactspace(poly, poly, 0.2 * (getCircle(poly).second + getCircle(poly).second)); SVMLearner learner; learner.setC(50); learner.setScaler(contactspace.getScaler()); learner.setUseScaler(true); learner.setGamma(50); std::vector<ContactSpaceSampleData> contactspace_samples = contactspace.uniform_sample(100000); std::ofstream scaler_file("scaler.txt"); scaler_file << contactspace.getScaler() << std::endl; learner.learn(contactspace_samples, contactspace.active_data_dim()); learner.save("model.txt"); std::cout << empiricalErrorRatio(contactspace_samples, learner) << " " << errorRatioOnGrid(contactspace, learner, 20) << std::endl; Collider2D collider(poly, poly); // flann::HierarchicalClusteringIndex<ContactSpaceSE2::DistanceType>* query_index = learner.constructIndexOfSupportVectorsForQuery<ContactSpaceSE2, flann::HierarchicalClusteringIndex, flann::HierarchicalClusteringIndexParams>(); int knn_k = 2; // 50; std::vector<ContactSpaceSampleData> support_samples; learner.collectSupportVectors(support_samples); ExtendedModel<ContactSpaceSE2, flann::HierarchicalClusteringIndex> extended_model = constructExtendedModelForModelDecisionBoundary<ContactSpaceSE2, SVMLearner, flann::HierarchicalClusteringIndex, flann::HierarchicalClusteringIndexParams>(contactspace, learner, support_samples, 0.01, knn_k); for(std::size_t i = 1001; i < 9999; ++i) { std::stringstream ss; ss << i; std::string ret; ss >> ret; std::size_t len = ret.length(); for(std::size_t j = 0; j < 4 - len; ++j) ret = "0" + ret; std::string filename = base_name + ret + ".txt"; std::string PD_file_name= std::string("PD_APD") + ret + ".txt"; std::ofstream PD_file(PD_file_name.c_str()); std::vector<std::vector<std::pair<std::string, DataVector> > > frame = readDumpFile(filename); double timing_per_frame = 0; for(std::size_t j = 0; j < frame.size(); ++j) { std::cout << i << " " << j << std::endl; if(frame[j][0].first == "Wall" || frame[j][1].first == "Wall") continue; boost::timer t; DataVector q = relative2D(frame[j][0].second, frame[j][1].second); if(collider.isCollide(q)) { // QueryResult PD_result = PD_query(learner, contactspace, query_index, q); QueryResult PD_result = PD_query(learner, contactspace, extended_model.index, extended_model.samples, q); PD_file << PD_result.PD << " "; } else { PD_file << 0 << " "; } timing_per_frame += t.elapsed(); } timing_file << timing_per_frame << " "; timing_file.flush(); PD_file.flush(); } }