void CollisionDestroyPositioner::affect(float ms)
{
Entity *entity = getEntity();
CollisionDetector *collisionDetector =
entity->getLevel()->getCollisionDetector();
/* Convert vectors to ellipse space. */
Vector2D position = entity->getPosition();
Vector2D velocity = entity->getVelocity() * ms;
/* Used later. */
float time;
Vector2D collision;
if(collisionDetector->getClosestCollision(
entity->getRadius(), position, velocity,
&time, &collision)) {
entity->setGarbage();
entity->setPosition(collision);
} else {
entity->setPosition(position + velocity);
}
velocity = entity->getVelocity() + getGravity() * ms;
entity->setVelocity(velocity);
}
/*
Method is responsible for runtime checks:
- Collision detection
- Food eaten
- Drawing
Return(true) -> GameOver
Return(false)-> Keep running
*/
bool GameController::performGameRuntimeChecks(CInputManager &inputManager,
unsigned long &score, CollisionDetector &collisionDetector,
DrawEngine drawEngine, int key, Snake &snake, vector<Cell> &foodList,
FoodProvider &foodProvider, Cell lastCell)
{
//If collision detected
if(collisionDetector.collisionDetected(snake))
{
char message[] = "GAME OVER! <Press any key>";
drawEngine.showMessage(message);
inputManager.GetNextKeyWait(&key);
return true;
}
//Food eaten
else if(collisionDetector.foodEaten(snake, foodList))
{
//Setting new score
score += snake.getCells().size()*2;
//Draw score
drawEngine.updateScore(score);
//Add cell
snake.addCell();
//Recieving new foodList
vector<Cell> &list = foodProvider.provideFood(snake, drawEngine.getWindownWidth(),
drawEngine.getMenuBorderHeight());
foodList = list;
}
//Draw snake
drawEngine.drawSnake(snake, lastCell);
//Draw food
drawEngine.drawFood(foodList);
return false;
}
void GameController::initGame()
{
//Initializing variables
DrawEngine drawEngine;
CInputManager inputManager;
CLoopTimer loopTimer;
CollisionDetector collisionDetector;
FoodProvider foodProvider;
GameStatus gameStatus;
//Initializing console
drawEngine.initialize();
collisionDetector.initialize(drawEngine.getWindownWidth(), drawEngine.getMenuBorderHeight());
//Set start values
gameStatus.setGameStatus(gameStatus.MAIN_MENU);
unsigned long score = 0;
while(gameStatus.getGameStatus() != gameStatus.QUIT)
{
//Runs main menu
if(gameStatus.getGameStatus() == gameStatus.MAIN_MENU)
{
drawEngine.showMenu(inputManager, gameStatus);
}
//Run game
else
{
score = 0;
runGame(inputManager, loopTimer, score, collisionDetector, foodProvider, drawEngine);
gameStatus.setGameStatus(gameStatus.MAIN_MENU);
}
}
}
void Player::handleDestructableCollisions(std::vector<Destructable*> destructables, Camera* camera)
{
CollisionDetector detector;
for(int i = 0; i < destructables.size(); ++i)
{
for(int j = 0; j < collisionPoints.size(); ++j)
{
CollisionPoint before = collisionPoints.at(j);
CollisionPoint translatedPoint(before.x + movementVector.x, before.y + movementVector.y, before.type);
if (detector.checkCollision(translatedPoint, destructables.at(i), camera))
{
switch (collisionPoints.at(j).type)
{
case HEAD:
movementVector.y = destructables.at(i)->getBoundingBox().y + destructables.at(i)->getBoundingBox().h + 1;
break;
case FEET:
movementVector.y = destructables.at(i)->getBoundingBox().y - m_h;
m_airbourne = false;
break;
case LEFT:
movementVector.x = destructables.at(i)->getBoundingBox().x + destructables.at(i)->getBoundingBox().w + 1; // + camera->getOffsetX();
break;
case RIGHT:
movementVector.x = destructables.at(i)->getBoundingBox().x - m_w - 1; // + camera->getOffsetX();
break;
}
}
}
}
}
void cppCollisionDetection(double delta)
{
s0 = new Surface();
s1 = new Surface();
h0 = new Hierarchy();
h1 = new Hierarchy();
s0->Add(0,0,0,1,0);
s0->CreateSurface();
s1->Add(0,0,0,1,0);
s1->CreateSurface();
Triple t0(-delta,0,0);
Triple t1(delta,0,0);
RotationMatrix m0,m1;
h0->Initialize(s0);
h0->Behavior(&m0,&t0);
h1->Initialize(s1);
h1->Behavior(&m1,&t1);
CollisionDetector cd;
cd.CheckCollision(h0,h1);
cd.Select(1);
s->CreateSurface(s0,s1,delta);
delete h0;
delete h1;
delete s0;
delete s1;
}
TEST_F(CollisionDetectorTest, projectile_projectile) {
CollisionDetector detector;
NiceMock<MockGameObject> proj_1;
NiceMock<MockHitbox> proj_1_hitbox;
NiceMock<MockGameObject> proj_2;
NiceMock<MockHitbox> proj_2_hitbox;
ON_CALL(proj_1, hitbox()).WillByDefault(ReturnRef(proj_1_hitbox));
ON_CALL(proj_1_hitbox, is_overlapping(Ref(proj_2_hitbox))).WillByDefault(Return(true));
ON_CALL(proj_2, hitbox()).WillByDefault(ReturnRef(proj_2_hitbox));
ON_CALL(proj_2_hitbox, is_overlapping(Ref(proj_1_hitbox))).WillByDefault(Return(true));
EXPECT_CALL(proj_1, notify_collision(Ref(proj_2))).Times(0);
EXPECT_CALL(proj_2, notify_collision(Ref(proj_1))).Times(0);
detector.add_projectile(Team::FRIENDLY, proj_1);
detector.add_projectile(Team::ENEMY, proj_2);
detector.check_collisions();
}
TEST_F(CollisionDetectorTest, projectile_gameobject_no_collision) {
CollisionDetector detector;
NiceMock<MockGameObject> projectile;
NiceMock<MockHitbox> proj_hitbox;
NiceMock<MockGameObject> gameobj;
NiceMock<MockHitbox> gameobj_hitbox;
ON_CALL(projectile, hitbox()).WillByDefault(ReturnRef(proj_hitbox));
ON_CALL(proj_hitbox, is_overlapping(Ref(gameobj_hitbox))).WillByDefault(Return(false));
ON_CALL(gameobj, hitbox()).WillByDefault(ReturnRef(gameobj_hitbox));
ON_CALL(gameobj_hitbox, is_overlapping(Ref(proj_hitbox))).WillByDefault(Return(false));
EXPECT_CALL(projectile, notify_collision(Ref(gameobj))).Times(0);
EXPECT_CALL(gameobj, notify_collision(Ref(projectile))).Times(0);
detector.add_projectile(Team::FRIENDLY, projectile);
detector.add_game_object(Team::ENEMY, gameobj);
detector.check_collisions();
}
TEST_F(CollisionDetectorTest, gameobject_gameobject_no_collision) {
CollisionDetector detector;
NiceMock<MockGameObject> gameobj_1;
NiceMock<MockHitbox> hitbox_1;
NiceMock<MockGameObject> gameobj_2;
NiceMock<MockHitbox> hitbox_2;
ON_CALL(gameobj_1, hitbox()).WillByDefault(ReturnRef(hitbox_1));
ON_CALL(hitbox_1, is_overlapping(Ref(hitbox_2))).WillByDefault(Return(false));
ON_CALL(gameobj_2, hitbox()).WillByDefault(ReturnRef(hitbox_2));
ON_CALL(hitbox_2, is_overlapping(Ref(hitbox_1))).WillByDefault(Return(false));
EXPECT_CALL(gameobj_1, notify_collision(Ref(gameobj_2))).Times(0);
EXPECT_CALL(gameobj_2, notify_collision(Ref(gameobj_1))).Times(0);
detector.add_game_object(Team::FRIENDLY, gameobj_1);
detector.add_game_object(Team::ENEMY, gameobj_2);
detector.check_collisions();
}
void SimpleCollisionHandler::handleCollisions(TwoDScene &scene, CollisionDetector &detector, const VectorXs &oldpos, VectorXs &oldvel, scalar dt)
{
class SimpleCollisionCallback : public DetectionCallback
{
public:
SimpleCollisionCallback(TwoDScene &scene, SimpleCollisionHandler &handler) : scene(scene), handler(handler) {}
virtual void ParticleParticleCallback(int idx1, int idx2)
{
VectorXs n(2);
if(handler.detectParticleParticle(scene, idx1, idx2, n))
{
handler.addParticleParticleImpulse(idx1, idx2, n, 0);
handler.respondParticleParticle(scene, idx1, idx2, n);
}
}
virtual void ParticleEdgeCallback(int vidx, int eidx)
{
VectorXs n(2);
if(handler.detectParticleEdge(scene, vidx, eidx, n))
{
handler.addParticleEdgeImpulse(vidx, eidx, n, 0);
handler.respondParticleEdge(scene, vidx, eidx, n);
}
}
virtual void ParticleHalfplaneCallback(int vidx, int hidx)
{
VectorXs n(2);
if(handler.detectParticleHalfplane(scene, vidx, hidx, n))
{
handler.addParticleHalfplaneImpulse(vidx, hidx, n, 0);
handler.respondParticleHalfplane(scene, vidx, hidx, n);
}
}
TwoDScene &scene;
SimpleCollisionHandler &handler;
};
SimpleCollisionCallback callback(scene, *this);
detector.performCollisionDetection(scene, scene.getX(), scene.getX(), callback);
}
bool checkCollisions(Device::Ptr device, const State &state, const CollisionDetector &detector, const Q &q) {
State testState;
CollisionDetector::QueryResult data;
bool colFrom;
testState = state;
device->setQ(q,testState);
colFrom = detector.inCollision(testState,&data);
if (colFrom) {
cerr << "Configuration in collision: " << q << endl;
cerr << "Colliding frames: " << endl;
FramePairSet fps = data.collidingFrames;
for (FramePairSet::iterator it = fps.begin(); it != fps.end(); it++) {
cerr << (*it).first->getName() << " " << (*it).second->getName() << endl;
}
return false;
}
return true;
}
int main(int argc, char** argv) {
// Check arguments
if (argc < 5) {
std::cerr << "Usage: " << argv[0] << " NODES_FILE TETS_FILE\n";
exit(1);
}
// Construct the first mesh
MeshType mesh;
//construct the second mesh
MeshType mesh2;
std::vector<typename MeshType::node_type> mesh_node;
// Read all Points and add them to the Mesh
std::ifstream nodes_file(argv[1]);
Point p;
while (CS207::getline_parsed(nodes_file, p)) {
mesh_node.push_back(mesh.add_node(p));
}
// Read all mesh triangles and add them to the Mesh
std::ifstream tris_file(argv[2]);
std::array<int,3> t;
while (CS207::getline_parsed(tris_file, t)) {
mesh.add_triangle(mesh_node[t[0]], mesh_node[t[1]], mesh_node[t[2]]);
}
std::vector<typename MeshType::node_type> mesh_node2;
// Read all Points and add them to the Mesh
std::ifstream nodes_file2(argv[3]);
while (CS207::getline_parsed(nodes_file2, p)) {
mesh_node2.push_back(mesh2.add_node(p));
}
// Read all mesh triangles and add them to the Mesh
std::ifstream tris_file2(argv[4]);
while (CS207::getline_parsed(tris_file2, t)) {
mesh2.add_triangle(mesh_node2[t[0]], mesh_node2[t[1]], mesh_node2[t[2]]);
}
//move the second mesh to the specified position
for(auto it = mesh2.node_begin();it!=mesh2.node_end();++it){
(*it).position().elem[1] +=4 ;
(*it).position().elem[2] +=4 ;
}
// Print out the stats
std::cout << mesh.num_nodes() << " "
<< mesh.num_edges() << " "
<< mesh.num_triangles() << std::endl;
std::cout << mesh2.num_nodes() << " "
<< mesh2.num_edges() << " "
<< mesh2.num_triangles() << std::endl;
//set the mass and velocity of each Node for the first mesh
for (auto it = mesh.node_begin(); it != mesh.node_end(); ++it){
(*it).value().mass = float(1)/mesh.num_nodes();
(*it).value().velocity = Point(0, 10, 10);
}
//set the mass and velocity of each Node for the second mesh
for (auto it = mesh2.node_begin(); it != mesh2.node_end(); ++it){
(*it).value().mass = float(1)/mesh.num_nodes();
(*it).value().velocity = Point(0, -10, -10);
}
//set K and L for each edge
for (auto it = mesh.node_begin(); it != mesh.node_end(); ++it)
{
for (auto j = (*it).edge_begin(); j != (*it).edge_end(); ++j){
(*j).value().L = (*j).length();
(*j).value().K = 16000;
}
}
for (auto it = mesh2.node_begin(); it != mesh2.node_end(); ++it)
{
for (auto j = (*it).edge_begin(); j != (*it).edge_end(); ++j){
(*j).value().L = (*j).length();
(*j).value().K = 16000;
}
}
// Launch the SDLViewer
CS207::SDLViewer viewer;
auto node_map = viewer.empty_node_map(mesh);
viewer.launch();
viewer.add_nodes(mesh.node_begin(), mesh.node_end(), node_map);
viewer.add_edges(mesh.edge_begin(), mesh.edge_end(), node_map);
viewer.add_nodes(mesh2.node_begin(), mesh2.node_end(), node_map);
viewer.add_edges(mesh2.edge_begin(), mesh2.edge_end(), node_map);
viewer.center_view();
//Begin the mass-spring simulation
double dt = 0.0002;
double t_start = 0.0;
double t_end = 10.0;
//three color parameter
int color1 = 1;
int color2 = 1;
int color3 = 1;
//Create listener
Pause_listener* pause = new Pause_listener(dt);
Speed_listener* speed = new Speed_listener(dt, dt);
XYZ_listener<MeshType>* xyz = new XYZ_listener<MeshType>(&mesh);
Color_listener* col = new Color_listener(&color1, &color2, &color3);
//add listener
viewer.add_listener(pause);
viewer.add_listener(speed);
viewer.add_listener(xyz);
viewer.add_listener(col);
//Initialize forces
WindForce wind_force(Point(0,100,200));
PressureForce<typename MeshType::node_type, MeshType> pressure_force(1, 2500, &mesh);
DampingForce damp_force(float(1)/mesh.num_nodes());
//auto force = make_combined_force(MassSpringForce(), GravityForce(), make_combined_force(pressure_force, damp_force, wind_force));
auto force = make_combined_force(MassSpringForce(), make_combined_force(pressure_force, damp_force, wind_force));
//Initialize constriants
auto constraint = PlaneConstraint<MeshType>(-4);
//simulation processing
for (double t = t_start; t < t_end; t += dt) {
constraint(mesh, 0);
constraint(mesh2, 0);
//define a collision constrain
auto collision_constrain = CollisionConstraint<MeshType>();
//add the forces to the meshs at each dt
symp_euler_step(mesh, t, dt, force);
symp_euler_step(mesh2, t, dt, force);
//detec the collision betweent the two meshes
CollisionDetector<MeshType> c;
c.add_object(mesh);
c.add_object(mesh2);
c.check_collisions();
std::vector<unsigned> collision;
std::vector<unsigned> collision2;
//find the corresponding mesh for each node
for (auto it=c.begin(); it!= c.end(); ++it){
auto boom = *it;
Node n = boom.n1;
if (boom.mesh1 == &mesh)
collision.push_back(n.index());
if (boom.mesh1 == &mesh2)
collision2.push_back(n.index());
}
//add the collision constrain to the meshes
collision_constrain(mesh,mesh2,collision,collision2);
viewer.set_label(t);
//update with removed nodes
//clear teh viewer's node
viewer.clear();
node_map.clear();
//update viewer with new positions and new edges
viewer.add_nodes(mesh.node_begin(), mesh.node_end(), color(color1, color2, color3), node_map);
viewer.add_edges(mesh.edge_begin(), mesh.edge_end(), node_map);
viewer.add_nodes(mesh2.node_begin(), mesh2.node_end(), color(color1, color2, color3), node_map);
viewer.add_edges(mesh2.edge_begin(), mesh2.edge_end(), node_map);
// These lines slow down the animation for small graphs
if (mesh.num_nodes() < 100)
CS207::sleep(0.001);
}
return 0;
}
int main(int argc, char* args[]) {
Renderer* renderer = new Renderer();
if(!renderer->init()) {
printf( "Failed to initialize!\n" );
} else {
Character* c = new Character(new Texture("res/img/dot.bmp", 200, 200));
renderer->addTexture(new Texture("res/img/waterlevel2.png", 0, 0));
renderer->render();
renderer->addTexture(c->getTexture());
bool quit = false;
SDL_Event e;
LoadCollisionMap lcm;
list<Circle> circlesList;
circlesList = lcm.load();
list<Circle> l;
Circle c1(200,0,10);
l.push_back(c1);
CollisionDetector cd;
int y = 0;
while(!cd.hasCollision(circlesList, l)) {
l.pop_back();
y += 1;
Circle c1(200,y,10);
l.push_back(c1);
}
y -= 1;
c->setPosX(200);
c->setPosY(y);
renderer->render();
int mVelX = 0;
int DOT_VEL = 1;
while(!quit) {
while(SDL_PollEvent(&e) != 0) {
if(e.type == SDL_QUIT) {
quit = true;
}
if(e.type == SDL_KEYDOWN && e.key.repeat == 0) {
switch(e.key.keysym.sym) {
case SDLK_LEFT: mVelX -= DOT_VEL; break;
case SDLK_RIGHT: mVelX += DOT_VEL; break;
}
} else if(e.type == SDL_KEYUP && e.key.repeat == 0) {
switch(e.key.keysym.sym) {
case SDLK_LEFT: mVelX += DOT_VEL; break;
case SDLK_RIGHT: mVelX -= DOT_VEL; break;
}
}
}
c->setPosX(c->getPosX() + mVelX);
LoadCollisionMap lcm;
list<Circle> circlesList;
circlesList = lcm.load();
int mPosX = c->getPosX();
int mPosY = c->getPosY();
list<Circle> l;
Circle c1(mPosX,mPosY,10);
l.push_back(c1);
CollisionDetector cd;
int y = mPosY;
while(cd.hasCollision(circlesList, l)) {
l.pop_back();
y -= 1;
Circle c1(mPosX,y,10);
l.push_back(c1);
};
while(!cd.hasCollision(circlesList, l)) {
l.pop_back();
y += 1;
Circle c1(mPosX,y,10);
l.push_back(c1);
};
c->setPosY(y);
renderer->render();
}
}
return 0;
}
void GameEngine::run()
{
spriteVec.push_back(levelVec.at(currentLevel)->getBackground());
spriteVec.push_back(player);
bool goOn = true;
long laps = 0;
while (goOn) {
Uint32 nextTick = SDL_GetTicks() + ticks;
SDL_RenderClear(win->getRen());
if (initialLoop) {
textInput();
initialLoop = false;
}
SDL_Event eve2;
for (size_t i = 0; i < spriteVec.size(); i++) {
spriteVec.at(i)->draw();
cd.checkCollisions(spriteVec.at(i));
}
removeSprites();
cd.clearVector();
for (size_t i = 0; i < textBoxVec.size(); i++) {
textBoxVec.at(i)->draw();
}
if (pointCounter != nullptr) {
pointCounter->draw();
}
SDL_RenderPresent(win->getRen());
if (player == nullptr) {
playerDied();
}
while (SDL_PollEvent(&eve2)) {
switch (eve2.type) {
case SDL_QUIT: goOn = false;
break;
case SDL_KEYDOWN:
if (functionMap.count(eve2.key.keysym.sym) != 0) {
functionMap[eve2.key.keysym.sym]();
}
for (size_t i = 0; i < memberFuncVec.size(); i++) {
memberFuncVec.at(i)->perform(eve2.key.keysym.sym);
}
for (size_t i = 0; i < spriteVec.size(); i++) {
spriteVec.at(i)->draw(eve2);
}
break;
case SDL_KEYUP:
for (size_t i = 0; i < spriteVec.size(); i++) {
spriteVec.at(i)->draw(eve2);
}
break;
}
}
laps++;
levelVec.at(currentLevel)->spawnWave(this, laps);
deleteSpawns(laps);
//This is triggered when the level is over:
if (laps > levelVec.at(currentLevel)->getEndFrame()) {
SDL_RenderClear(win->getRen());
for (size_t i = 0; i < spriteVec.size(); i++) {
if (spriteVec.at(i)->getIsPlayer() == false) {
spriteVec.at(i)->destroy(); //Prepare every sprite for removal except the player
}
}
removeSprites();
spriteVec.clear();
deleteAllSpawns();
SDL_RenderClear(win->getRen());
levelSwap(); //Needs to be set by the game programmer
currentLevel++;
return this->run();
}
if (SDL_GetTicks() < nextTick) {
SDL_Delay(nextTick - SDL_GetTicks());
}
}
}
//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& _fileName)
{
using namespace std;
using namespace Eigen;
using namespace dart::math;
using namespace dart::collision;
using namespace dart::constraint;
using namespace dart::dynamics;
using namespace dart::simulation;
using namespace dart::utils;
//----------------------------------------------------------------------------
// Settings
//----------------------------------------------------------------------------
// Number of random state tests for each skeletons
#ifndef NDEBUG // Debug mode
// size_t testCount = 1;
#else
// size_t testCount = 1;
#endif
World* world = new World;
EXPECT_TRUE(world != NULL);
world->setGravity(Vector3d(0.0, -10.00, 0.0));
world->setTimeStep(0.001);
world->getConstraintSolver()->setCollisionDetector(
new DARTCollisionDetector());
Skeleton* sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
BodyNode* sphere = sphereSkel->getBodyNode(0);
Joint* sphereJoint = sphere->getParentJoint();
sphereJoint->setVelocity(3, random(-2.0, 2.0)); // x-axis
sphereJoint->setVelocity(5, random(-2.0, 2.0)); // z-axis
world->addSkeleton(sphereSkel);
EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
assert(sphere);
Skeleton* boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
Vector3d(0.0, 1.0, 0.0));
BodyNode* box = boxSkel->getBodyNode(0);
Joint* boxJoint = box->getParentJoint();
boxJoint->setVelocity(3, random(-2.0, 2.0)); // x-axis
boxJoint->setVelocity(5, random(-2.0, 2.0)); // z-axis
// world->addSkeleton(boxSkel);
// EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
// assert(box);
Skeleton* groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
Vector3d(0.0, -0.05, 0.0));
groundSkel->setMobile(false);
// BodyNode* ground = groundSkel->getBodyNode(0);
world->addSkeleton(groundSkel);
EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
// assert(ground);
EXPECT_EQ(world->getNumSkeletons(), 2);
ConstraintSolver* cs = world->getConstraintSolver();
CollisionDetector* cd = cs->getCollisionDetector();
// Lower and upper bound of configuration for system
// double lb = -1.5 * DART_PI;
// double ub = 1.5 * DART_PI;
int maxSteps = 500;
for (int i = 0; i < maxSteps; ++i)
{
// Vector3d pos1 = sphere->getWorldTransform().translation();
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
cd->detectCollision(true, true);
if (cd->getNumContacts() == 0)
{
world->step();
continue;
}
// for (size_t j = 0; j < cd->getNumContacts(); ++j)
// {
// Contact contact = cd->getContact(j);
// Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// }
world->step();
for (size_t j = 0; j < cd->getNumContacts(); ++j)
{
Contact contact = cd->getContact(j);
Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "pos1[1]: " << pos1[1] << std::endl;
// std::cout << "pos1:" << pos1.transpose() << std::endl;
std::cout << "vel1:" << vel1.transpose() << ", pos1[1]: " << pos1[1] << std::endl;
// EXPECT_NEAR(pos1[0], 0.0, 1e-9);
// EXPECT_NEAR(pos1[1], -0.05, 1e-2);
// EXPECT_NEAR(pos1[2], 0.0, 1e-9);
// EXPECT_NEAR(vel1[0], 0.0, 1e-9);
// EXPECT_NEAR(vel1[1], 0.0, 1e-9);
// EXPECT_NEAR(vel1[2], 0.0, 1e-9);
// if (!equals(vel1, Vector3d(0.0, 0.0, 0.0)))
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// EXPECT_EQ(vel1, Vector3d::Zero());
}
// std::cout << std::endl;
break;
}
delete world;
}
/**
@return top of the geometry indices assigned to the body.
The geometry id corresponding to a link is <the top index> + <link->index()>.
*/
int cnoid::addBodyToCollisionDetector(Body& body, CollisionDetector& detector, bool enableSelfCollisions)
{
const int idTop = detector.numGeometries();
const int numLinks = body.numLinks();
int excludeTreeDepth = 3;
boost::dynamic_bitset<> exclusions(numLinks);
boost::dynamic_bitset<> staticFlags(numLinks);
const Mapping& cdInfo = *body.info()->findMapping("collisionDetection");
if(cdInfo.isValid()){
excludeTreeDepth = cdInfo.get("excludeTreeDepth", excludeTreeDepth);
const Listing& excludeLinks = *cdInfo.findListing("excludeLinks");
for(int i=0; i < excludeLinks.size(); ++i){
Link* link = body.link(excludeLinks[i].toString());
if(link){
exclusions[link->index()] = true;
}
}
}
if(body.isStaticModel() || body.rootLink()->isFixedJoint()){
setStaticFlags(body.rootLink(), staticFlags);
}
for(int i=0; i < numLinks; ++i){
if(exclusions[i]){
detector.addGeometry(0);
} else {
int id = detector.addGeometry(body.link(i)->collisionShape());
if(staticFlags[i]){
detector.setGeometryStatic(id);
}
}
}
if(!enableSelfCollisions){
// exclude all the self link pairs
for(int i=0; i < numLinks; ++i){
if(!exclusions[i]){
for(int j=i+1; j < numLinks; ++j){
if(!exclusions[j]){
detector.setNonInterfarenceGeometyrPair(i + idTop, j + idTop);
}
}
}
}
} else {
// exclude the link pairs whose distance in the tree is less than excludeTreeDepth
for(int i=0; i < numLinks; ++i){
if(!exclusions[i]){
Link* link1 = body.link(i);
for(int j=i+1; j < numLinks; ++j){
if(!exclusions[j]){
Link* link2 = body.link(j);
Link* parent1 = link1;
Link* parent2 = link2;
for(int k=0; k < excludeTreeDepth; ++k){
if(parent1){
parent1 = parent1->parent();
}
if(parent2){
parent2 = parent2->parent();
}
if(!parent1 && !parent2){
break;
}
if(parent1 == link2 || parent2 == link1){
detector.setNonInterfarenceGeometyrPair(i + idTop, j + idTop);
}
}
}
}
}
}
}
return idTop;
}
