void Player::Update(float dt)
{
float max_speed = 10;
b2Vec2 v = body->GetLinearVelocity();
if(move_left)
{
float mdiff = max_speed + v.x;
v.x -= mdiff*10*dt;
}
if(move_right)
{
float mdiff = max_speed - v.x;
v.x += mdiff*10*dt;
}
b2ContactEdge* contactedge = body->GetContactList();
b2Vec2 contact_normal(0, 0);
int contacts = 0;
bool touching_ground = false;
while(contactedge)
{
if(contactedge->contact->IsTouching() && contactedge->contact->IsEnabled())
{
touching_ground = true;
contact_normal += contactedge->contact->GetManifold()->localNormal;
contacts ++;
}
contactedge = contactedge->next;
}
contact_normal.x = contact_normal.x/contacts;
contact_normal.y = contact_normal.y/contacts;
//Jumping
if(jump && touching_ground)
{
jump = false;
v += 15*contact_normal;
}
//Thrust
if(jetpack)
{
v.y -= 180*dt;
}
body->SetLinearVelocity(v);
}
int collision_detection::refineContactNormals(const World::ObjectConstPtr& object,
CollisionResult &res,
double cell_bbx_search_distance,
double allowed_angle_divergence,
bool estimate_depth,
double iso_value,
double metaball_radius_multiple)
{
if(!object)
{
logError("No valid Object passed in, cannot refine Normals!");
return 0;
}
if(res.contact_count < 1)
{
logWarn("There do not appear to be any contacts, so there is nothing to refine!");
return 0;
}
int modified = 0;
// iterate through contacts
for( collision_detection::CollisionResult::ContactMap::iterator it = res.contacts.begin(); it != res.contacts.end(); ++it)
{
std::string contact1 = it->first.first;
std::string contact2 = it->first.second;
std::string octomap_name = "";
std::vector<collision_detection::Contact>& contact_vector = it->second;
if( contact1.find("octomap") != std::string::npos ) octomap_name = contact1;
else if( contact2.find("octomap") != std::string::npos ) octomap_name = contact2;
else
{
continue;
}
double cell_size = 0;
if(!object->shapes_.empty())
{
const shapes::ShapeConstPtr& shape = object->shapes_[0];
boost::shared_ptr<const shapes::OcTree> shape_octree = boost::dynamic_pointer_cast<const shapes::OcTree>(shape);
if(shape_octree)
{
boost::shared_ptr<const octomap::OcTree> octree = shape_octree->octree;
cell_size = octree->getResolution();
for(size_t contact_index = 0; contact_index < contact_vector.size(); contact_index++)
{
const Eigen::Vector3d& point = contact_vector[contact_index].pos;
const Eigen::Vector3d& normal = contact_vector[contact_index].normal;
octomath::Vector3 contact_point(point[0], point[1], point[2]);
octomath::Vector3 contact_normal(normal[0], normal[1], normal[2]);
octomath::Vector3 diagonal = octomath::Vector3(1,1,1);
octomath::Vector3 bbx_min = contact_point - diagonal*cell_size*cell_bbx_search_distance;
octomath::Vector3 bbx_max = contact_point + diagonal*cell_size*cell_bbx_search_distance;
octomap::point3d_list node_centers;
octomap::OcTreeBaseImpl<octomap::OcTreeNode, octomap::AbstractOccupancyOcTree>::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbx_min, bbx_max);
octomap::OcTreeBaseImpl<octomap::OcTreeNode, octomap::AbstractOccupancyOcTree>::leaf_bbx_iterator leafs_end = octree->end_leafs_bbx();
int count = 0;
for( ; it != leafs_end; ++it)
{
octomap::point3d pt = it.getCoordinate();
//double prob = it->getOccupancy();
if(octree->isNodeOccupied(*it)) // magic number!
{
count++;
node_centers.push_back(pt);
//logInform("Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]", count, prob, pt.x(), pt.y(), pt.z());
}
}
//logInform("Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells %d",
// contact_point.x(), contact_point.y(), contact_point.z(), cell_size, count);
//octree->getOccupiedLeafsBBX(node_centers, bbx_min, bbx_max);
//logError("bad stuff in collision_octomap_filter.cpp; need to port octomap call for groovy");
octomath::Vector3 n;
double depth;
if(getMetaballSurfaceProperties(node_centers, cell_size, iso_value, metaball_radius_multiple,
contact_point, n, depth, estimate_depth))
{
// only modify normal if the refinement predicts a "very different" result.
double divergence = contact_normal.angleTo(n);
if(divergence > allowed_angle_divergence)
{
modified++;
// logInform("Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
// divergence,
// contact_normal.x(), contact_normal.y(), contact_normal.z(),
// n.x(), n.y(), n.z());
contact_vector[contact_index].normal = Eigen::Vector3d(n.x(), n.y(), n.z());
}
if(estimate_depth)
contact_vector[contact_index].depth = depth;
}
}
}
}
}
return modified;
}
