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