Пример #1
0
glm::vec3 CarrierModelComponent::getSpawnPoint() const
{
	// TODO: Clean up after transform is made better.
	CaffComp::TransformComponent *transform = getOwner()->getTransform();
	CaffComp::PhysicsComponent   *physics	= getOwner()->getRigidBody();
	
	const btVector3 fwd = physics->getRigidBody()->getWorldTransform().getBasis().getColumn(2);
	
	const glm::vec3 forward(fwd.x(), fwd.y(), fwd.z());
	const glm::vec3 result = transform->getPosition() + (m_spawnPoint - forward);
	
	return result;
}
Пример #2
0
void boundaryConstrain(int i)
{
    btFluidParticles& particles = fluid->internalGetParticles();
    btVector3 &velFluid = particles.m_vel[i];
    btVector3 &posFluid = particles.m_pos[i];
    
    collisionWithBound(velFluid, posFluid, btVector3( 1.0, 0.0, 0.0), posFluid.x() - minBound.x() - Constant.m_particleRadius);
    collisionWithBound(velFluid, posFluid, btVector3( -1.0, 0.0, 0.0), maxBound.x() - posFluid.x() - Constant.m_particleRadius);
    collisionWithBound(velFluid, posFluid, btVector3( 0.0,  1.0, 0.0), posFluid.y() - minBound.y() - Constant.m_particleRadius);
    collisionWithBound(velFluid, posFluid, btVector3( 0.0, -1.0, 0.0), maxBound.y() - posFluid.y() - Constant.m_particleRadius);
    collisionWithBound(velFluid, posFluid, btVector3( 0.0, 0.0,  1.0), posFluid.z() - minBound.z() - Constant.m_particleRadius);
    collisionWithBound(velFluid, posFluid, btVector3( 0.0, 0.0, -1.0), maxBound.z() - posFluid.z() - Constant.m_particleRadius);
}
Пример #3
0
 virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius, 
                              btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
 {
   double center_[3] = {center.x() / bulletWorldScalingFactor,
                        center.y() / bulletWorldScalingFactor,
                        center.z() / bulletWorldScalingFactor};
   double up_[3] = {up.x() / bulletWorldScalingFactor,
                    up.y() / bulletWorldScalingFactor,
                    up.z() / bulletWorldScalingFactor};
   double axis_[3] = {axis.x() / bulletWorldScalingFactor,
                      axis.y() / bulletWorldScalingFactor,
                      axis.z() / bulletWorldScalingFactor};
   double color_[3] = {color.x(), color.y(), color.z()};
   
   if(callbacks_.drawSpherePatch)
     (*callbacks_.drawSpherePatch)(center_, up_, axis_, radius / bulletWorldScalingFactor,
       minTh, maxTh, minPs, maxPs, color_, stepDegrees, arg_);
   else
     btIDebugDraw::drawSpherePatch(center / bulletWorldScalingFactor, up / bulletWorldScalingFactor,
       axis / bulletWorldScalingFactor, radius / bulletWorldScalingFactor,
       minTh, maxTh, minPs, maxPs, color, stepDegrees);
 }
Пример #4
0
// the center of editing takes place at HERE
// raise terrain DIR = 1, lower terrain DIR = -1
void terrain::terrainEdit(btVector3 here, int dir)
{
	float area = tTool->diameter();
	float amount = tTool->increment();
	
	if(area <= 0 || amount <= 0) return;
	
	amount *= dir;															// determine the direction of editing, up or down
	
	float xp = here.x();													// make sure the X location of the edit is over the terrain
    if(xp < 0) xp = 0;
    else if(xp > m_terrainSize.x()) xp = m_terrainSize.x();
	
	float yp = here.y();													// make sure the Y location of the edit is over the terrain
    if(yp < 0) yp = 0;
    else if(yp > m_terrainSize.y()) yp = m_terrainSize.y();
	
	for(int i=0; i<m_terrainVertexCount; i++)								// loop through the terrain vertex array
	{
        float x = (i % m_pixelSize.width()) * m_terrainSize.x()/m_pixelSize.width();// get the X and Y incides of the terrain vertices
        float y = (i / m_pixelSize.width()) * m_terrainSize.y()/m_pixelSize.height();

		float xd = xp - x;
		float yd = yp - y;

		float d = (float)sqrt((xd*xd) + (yd*yd));							// calculate the diameter of the area
		float a = (m_terrainVerts[i].z + amount) - (amount*(d / area));		// calculate the new height of the vertex
		
		if((dir == 1 && a > m_terrainVerts[i].z) || (dir == -1 && a < m_terrainVerts[i].z)) {
			m_terrainVerts[i].z = a;
			
			if(a > m_terrainMaxHeight) 
				m_terrainMaxHeight = a;
			else if(a < m_terrainMinHeight)
				m_terrainMinHeight = a;
			
			float avgheight = (m_terrainMaxHeight + m_terrainMinHeight)/2.0;
			
			if(a > avgheight) m_terrainColors[i].x = 0.8;			// set red color
			else m_terrainColors[i].x = 0.;
			m_terrainColors[i].y = 0.;								// set green color
			if(a < avgheight) m_terrainColors[i].z = 0.8;			// set blue color
			else m_terrainColors[i].z = 0.;
		}
	}
	
	buildNormals();

    this->terrainRefresh();
	m_terrainModified = true;
}
Пример #5
0
  virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, 
                       const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
  {
    double center_[3] = {center.x() / bulletWorldScalingFactor,
                         center.y() / bulletWorldScalingFactor,
                         center.z() / bulletWorldScalingFactor};
    double normal_[3] = {normal.x() / bulletWorldScalingFactor,
                         normal.y() / bulletWorldScalingFactor,
                         normal.z() / bulletWorldScalingFactor};
    double axis_[3] = {axis.x() / bulletWorldScalingFactor,
                       axis.y() / bulletWorldScalingFactor,
                       axis.z() / bulletWorldScalingFactor};
    double color_[3] = {color.x(), color.y(), color.z()};

    if(callbacks_.drawArc)
      (*callbacks_.drawArc)(center_, normal_, axis_,
        radiusA / bulletWorldScalingFactor, radiusB / bulletWorldScalingFactor,
        minAngle, maxAngle, color_, drawSect, stepDegrees, arg_);
    else
      btIDebugDraw::drawArc(center / bulletWorldScalingFactor, normal / bulletWorldScalingFactor,
        axis / bulletWorldScalingFactor, radiusA / bulletWorldScalingFactor, radiusB / bulletWorldScalingFactor,
        minAngle, maxAngle, color, drawSect, stepDegrees);
  }
Пример #6
0
void collider::on_collision(
    collision_controller *other,
    btVector3 point,
    btVector3 normal,
    btScalar depth)
{
    push_component();
    lua_getfield(L, -1, "on_collision");
    if(lua_isnil(L, -1))
        lua_pop(L, 1);
    else
    {
        collider *other_collider = dynamic_cast<collider*>(other);
        if(other_collider)
            other_collider->push_component();
        else
            lua_pushboolean(L, 0);
        l_pushvect(L, point.x(), point.y(), point.z());
        l_pushvect(L, normal.x(), normal.y(), normal.z());
        lua_pushnumber(L, depth);
        lua_call(L, 4, 0);
    }
    lua_pop(L, 1);
}
geometry_msgs::TransformStamped createTransform(btQuaternion q, btVector3 v, ros::Time stamp, const std::string& frame1, const std::string& frame2)
{
  geometry_msgs::TransformStamped t;
  t.header.frame_id = frame1;
  t.child_frame_id = frame2;
  t.header.stamp = stamp;
  t.transform.translation.x = v.x();
  t.transform.translation.y = v.y();
  t.transform.translation.z = v.z();
  t.transform.rotation.x = q.x();
  t.transform.rotation.y = q.y();
  t.transform.rotation.z = q.z();
  t.transform.rotation.w = q.w();
  return t;
}
    void btWorldFactory::createBoxesFloor(QVariantList &shapesList, double areaX, double areaZ, btVector3 pos, btVector3 boxMin, btVector3 boxMax) {

        // Boxes Floor
        double sizeX = Tools::random((double)boxMin.x(), (double)boxMax.x());
        double sizeZ = Tools::random((double)boxMin.z(), (double)boxMax.z());

        btVector3 recal(sizeX,0,sizeZ);
        pos = pos + recal;

        int nbBoxesX = areaX / sizeX;
        int nbBoxesZ = areaZ / sizeZ;

        for(int i=0;i<nbBoxesX;++i){
            for(int j=0;j<nbBoxesZ;++j){

                double sizeY = Tools::random((double)boxMin.y(), (double)boxMax.y());

                createBox(shapesList,
                          btVector3(sizeX,sizeY,sizeZ),
                          btVector3(i*sizeX - areaX*0.5 + pos.x(),sizeY/2.0 + pos.y(),j*sizeZ - areaZ*0.5 +  pos.z()),
                          btVector3(0,0,0),0);
            }
        }
    }
Пример #9
0
/**
* A cloud of smoke
**/
void EffectsManager::smokeCloud(const btVector3& location)
{
	SPK::Emitter* emitter = SPK::NormalEmitter::create();
	emitter->setZone(SPK::Sphere::create(SPK::Vector3D(location.x(), location.y(), location.z()), 2.0f));
	emitter->setFlow(-1);
	emitter->setTank(50);
	emitter->setForce(3.0f, 5.0f);

	SPK::Group* group = SPK::Group::create(model_smoke, 50);
	group->addEmitter(emitter);
	group->setGravity(gravity);
	group->setFriction(1.0f);
	group->setRenderer(render_smoke);
	st->addParticleGroup(group);
}
Пример #10
0
CellPosition Grid::getCellPosition(const btVector3& position) const
{
    int k1 = int((position.x() - m_minBoundary.x()) / m_gridCellSize);
    if (k1 < 0) k1 = 0;
    else if (k1 >= m_data.size()) k1 = m_data.size()-1;
    
    int k2 = int((position.y() - m_minBoundary.y()) / m_gridCellSize);
    if (k2 < 0) k2 = 0;
    else if (k2 >= m_data[0].size()) k2 = m_data[0].size()-1;
    
    int k3 = int((position.z() - m_minBoundary.z()) / m_gridCellSize);
    if (k3 < 0) k3 = 0;
    else if (k3 >= m_data[0][0].size()) k3 = m_data[0][0].size()-1;
    
    return CellPosition(k1, k2, k3);
}
Пример #11
0
void DrawTriangle(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& color)
{
//	glDisable(GL_LIGHTING);
	glColor4f(color.x(), color.y(), color.z(), 1.0f);
	btVector3 tmp[] = {p0, p1, p2};
	glEnableClientState(GL_VERTEX_ARRAY);
#ifndef BT_USE_DOUBLE_PRECISION
	glVertexPointer(3, GL_FLOAT, sizeof(btVector3), &tmp[0].x());
#else
	glVertexPointer(3, GL_DOUBLE, sizeof(btVector3), &tmp[0].x());
#endif
	glDrawArrays(GL_TRIANGLES, 0, 3);
	glDisableClientState(GL_VERTEX_ARRAY);
//	glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
//	glEnable(GL_LIGHTING);
}
Пример #12
0
/**
* Spray blood in all directions
**/
void EffectsManager::bloodSpray(const btVector3& location, float damage)
{
	SPK::RandomEmitter* emitter;
	SPK::Group* group;

	emitter = SPK::RandomEmitter::create();
	emitter->setZone(SPK::Point::create(SPK::Vector3D(location.x(), location.y() - 0.5f, location.z())));
	emitter->setFlow(-1);
	emitter->setTank(damage / 10.0f);
	emitter->setForce(5.0f, 10.0f);

	group = SPK::Group::create(model_blood, damage);
	group->addEmitter(emitter);
	group->setGravity(gravity);
	group->setRenderer(render_blood);
	st->addParticleGroup(group);
}
Пример #13
0
static void DrawLine(const btVector3& p0, const btVector3& p1, const btVector3& color, float line_width)
{
	glDisable(GL_LIGHTING);
	glLineWidth(line_width);
	glColor4f(color.x(), color.y(), color.z(), 1.0f);
	btVector3 tmp[] = {p0, p1};
	glEnableClientState(GL_VERTEX_ARRAY);
#ifndef BT_USE_DOUBLE_PRECISION
	glVertexPointer(3, GL_FLOAT, sizeof(btVector3), &tmp[0].x());
#else
	glVertexPointer(3, GL_DOUBLE, sizeof(btVector3), &tmp[0].x());
#endif
	glDrawArrays(GL_LINES, 0, 2);
	glDisableClientState(GL_VERTEX_ARRAY);
	glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
	glEnable(GL_LIGHTING);
}
Пример #14
0
btMatrix3x3 GetOrientedBasis(btVector3 const &z)
{
	btAssert(fabsf(z.length()-1) < 0.01f);
	btVector3 t(0,0,0);
	if(fabsf(z.z() < 0.999f))
	{
		t.setZ(1);
	}
	else
	{
		t.setX(1);
	}
	btVector3 x = t.cross(z).normalize();
	btVector3 y = z.cross(x).normalize();
	return btMatrix3x3(	x.x(), y.x(), z.x(),
						x.y(), y.y(), z.y(),
						x.z(), y.z(), z.z());
}
void TrackDisplay::createLine(Ogre::Vector3 pos, Ogre::Vector3 old_pos,
		Ogre::Vector3 scale, btVector3 color, std::vector<
				ogre_tools::BillboardLine*> &vec,bool add_cross) {

	ogre_tools::BillboardLine* lines = newBillboardLine();
	lines->setPosition(pos);
	lines->setOrientation(Ogre::Quaternion(1,0,0,0));
	lines->setScale(scale);

	lines->setColor(color.x(), color.y(), color.z(), alpha_);
	lines->clear();

	lines->setLineWidth(lineWidth_);
	lines->setMaxPointsPerLine(2);
	lines->setNumLines(4);

	Ogre::Vector3 v;
	if(add_cross) {
		for(int i=0;i<3;i++) {

			v = Ogre::Vector3(0, 0, 0);
			v[i] = lineWidth_ * 5;
			rviz::robotToOgre(v);
			lines->addPoint(v);

			v = Ogre::Vector3(0, 0, 0);
			v[i] = -lineWidth_ * 5;
			rviz::robotToOgre(v);
			lines->addPoint(v);
			lines->newLine();
		}
	}

	v = Ogre::Vector3(0, 0, 0);
	rviz::robotToOgre(v);
	lines->addPoint(v);

	v = old_pos - pos;
//	rviz::robotToOgre(v);
	lines->addPoint(v);

	vec.push_back(lines);
}
bool TrackDisplay::transform(const btTransform &pose, const btVector3 &scaleIn,
		Ogre::Vector3& pos, Ogre::Quaternion& orient, Ogre::Vector3& scaleOut) {

	pos = Ogre::Vector3(pose.getOrigin().x(), pose.getOrigin().y(),
			pose.getOrigin().z());
	rviz::robotToOgre(pos);

	btQuaternion quat;
	pose.getBasis().getRotation(quat);
	orient = Ogre::Quaternion::IDENTITY;
	rviz::ogreToRobot(orient);
	orient = Ogre::Quaternion(quat.w(), quat.x(), quat.y(), quat.z()) * orient;
	rviz::robotToOgre(orient);

	scaleOut = Ogre::Vector3(scaleIn.x(), scaleIn.y(), scaleIn.z());
	rviz::scaleRobotToOgre(scaleOut);

	return true;
}
Пример #17
0
 virtual void flushLines()
 {
     int sz = m_linePoints.size();
     if (sz)
     {
         float debugColor[4];
         debugColor[0] = m_currentLineColor.x();
         debugColor[1] = m_currentLineColor.y();
         debugColor[2] = m_currentLineColor.z();
         debugColor[3] = 1.f;
         m_glApp->m_renderer->drawLines(&m_linePoints[0].x,debugColor,
                                        m_linePoints.size(),sizeof(MyDebugVec3),
                                        &m_lineIndices[0],
                                        m_lineIndices.size(),
                                        1);
         m_linePoints.clear();
         m_lineIndices.clear();
     }
 }
Пример #18
0
	NxQuat(const float angle, const btVector3 & axis)
	{
	x = axis.x();
	y = axis.y();
	z = axis.z();

	const float i_length =  1.0f / sqrtf( x*x + y*y + z*z );
	x = x * i_length;
	y = y * i_length;
	z = z * i_length;

	float Half = degToRad(angle * 0.5f);

	w = cosf(Half);
	const float sin_theta_over_two = sinf(Half );
	x = x * sin_theta_over_two;
	y = y * sin_theta_over_two;
	z = z * sin_theta_over_two;
	}
void GameObjectStaticManager::CreateEntity(char * path, PhysicsEngine* engine, btQuaternion& orientation,
	btVector3& position, btScalar mass)
{
	std::string pathStr(path);
	string base = pathStr.erase(pathStr.find_last_of('/'));
	base.push_back('/');

	// Set object position
	glm::mat4x4 modelMatrix = mat4_cast(quat(orientation.w(), orientation.x(), orientation.y(), orientation.z()));
	modelMatrix = glm::translate(modelMatrix, glm::vec3(position.x(), position.y(), position.z()));

	// Get the mode pair
	std::pair<int, Model*>modelPair = resource::ModelManager::GetSingleton()->GetModel(modelMatrix, path);

	// If the first entry of the pair is -1 then the model has not yet been loaded in.
	if (modelPair.first == -1)
	{
		// Load the obj files
		std::vector<tinyobj::shape_t> shapes;
		std::vector<tinyobj::material_t> materials;
		tinyobj::LoadObj(shapes, materials, path, base.c_str());
		
		StaticPhysicsObject* physStart = engine->CreateStaticObject(shapes, orientation, position,
				btVector3(1, 1, 1), mass, path);

		Model* modelStart = resource::ModelManager::GetSingleton()->CreateModels(modelMatrix, shapes, materials, base.c_str(), path);
		new (&m_gameEntity[m_activeEntitys]) GameEntity(m_activeEntitys, shapes.size(), modelStart, physStart);
		m_activeEntitys++;
	}
	else
	{
		std::pair<int, StaticPhysicsObject*>physPair = engine->GetBody(path, orientation, position, btVector3(1, 1, 1), mass);
		new (&m_gameEntity[m_activeEntitys]) GameEntity(m_activeEntitys, modelPair.first, modelPair.second, physPair.second);
		m_activeEntitys++;
	}

#ifdef BUILDER_MODE
	std::string temp = GetName(path);
	temp.push_back('_' + m_entityMap.size());
	m_entityMap.insert(std::pair<string, std::pair<std::string, GameEntity*>>(temp, std::pair<std::string, GameEntity*>(path,&m_gameEntity[m_activeEntitys - 1])));
#endif
}
Пример #20
0
 btVector3	btBarrelShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
	btVector3 supVec(0,0,0);
	btVector3 supVecD;

	// suppport point on the lathed ellipse?
	btScalar pY = vec0.y();
	btScalar pR = std::sqrt (vec0.z()*vec0.z() + vec0.x()*vec0.x() );
	btScalar pH = pR;
	btScalar dX = vec0.x()/pR;
	btScalar dZ = vec0.z()/pR;
	btScalar tpar = std::atan((pY*R_vert)/(pH*R_hor));
	btScalar sY = R_vert * sin(tpar);
	btScalar sH = R_hor  * cos(tpar);
	btScalar sR = sH + R_offset;
	btScalar sX = dX * sR;
	btScalar sZ = dZ * sR;
	supVec.setValue(sX,sY,sZ);
	btScalar len = supVec.length();

	// support point on the top disc?
	if ((fabs(Y_high) < R_vert)&(supVec.y()>Y_high))
	{
		btScalar R_high_ellips = R_hor * sqrt( 1- pow( Y_high/R_vert ,2) );
		btScalar R_high = R_high_ellips + R_offset;
		btScalar rad_ratio = pR/R_high;
		supVecD.setValue(vec0.x()/rad_ratio, Y_high, vec0.z()/rad_ratio);
		supVec = supVecD;
	}
	// support point on the bottom disc?
	if ((fabs(Y_low) < R_vert)&(supVec.y()<Y_low))
	{
		btScalar R_low_ellips = R_hor * sqrt( 1- pow( Y_low/R_vert ,2) );
		btScalar R_low = R_low_ellips + R_offset;
		btScalar rad_ratio = pR/R_low;
		supVecD.setValue(vec0.x()/rad_ratio, Y_low, vec0.z()/rad_ratio);
		supVec = supVecD;
	}

	return supVec;
}
Пример #21
0
// tries out a grasp and returns the number of anchors attached
static int tryGrasp(const GraspingActionContext &ctx, int node, const btVector3 &gripperdir) {
    GraspingActionContext forkctx = ctx.fork();

    int startNumAnchors = ctx.cloth->softBody->m_anchors.size();

    stringstream ss; ss << "grab " << node << ' ' << gripperdir.x() << ' ' << gripperdir.y() << ' ' << gripperdir.z();

    try {
        Action::Ptr a = GraspingActionSpec(ss.str()).createAction(forkctx);

        while (!a->done()) {
            a->step(BulletConfig::dt);
            forkctx.env->step(BulletConfig::dt,
                    BulletConfig::maxSubSteps, BulletConfig::internalTimeStep);
        }
    } catch (const GraspingActionFailed &) {
        return 0;
    }

    return forkctx.cloth->softBody->m_anchors.size() - startNumAnchors;
}
Пример #22
0
/**
* It's an EXPLOSION!
**/
void EffectsManager::explosion(const btVector3& location, float damage)
{
	SPK::Emitter* emitter;
	SPK::Group* group;

	// Emitter
	emitter = SPK::RandomEmitter::create();
	emitter->setZone(SPK::Point::create(SPK::Vector3D(location.x(), location.y(), location.z())));
	emitter->setFlow(-1);
	emitter->setTank(100);
	emitter->setForce(10.0f, 20.0f);

	// Create group
	group = SPK::Group::create(model_fireball, 100);
	group->addEmitter(emitter);
	group->setGravity(gravity);
	group->setRenderer(render_fireball);
	st->addParticleGroup(group);
	
	this->smokeCloud(location);
}
Пример #23
0
void BulletCHOP::addBody(btVector3 pos, btVector3 rot, btVector3 scale, btScalar mass){

	btCollisionShape* colShape = new btBoxShape(0.5*scale);

	collisionShapes.push_back(colShape);
		
	/// Create Dynamic Objects
	btTransform startTransform;
	startTransform.setIdentity();

	//rigidbody is dynamic if and only if mass is non zero, otherwise static
	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		colShape->calculateLocalInertia(mass,localInertia);

	btMatrix3x3 rotMat;
	rotMat.setEulerZYX(rot.x(),rot.y(),rot.z());
		
	startTransform.setOrigin(pos);
	startTransform.setBasis(rotMat);

			
	//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
	btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);
					
	if(isDynamic == 0){
		body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
		body->setActivationState(DISABLE_DEACTIVATION);
	}

	dynamicsWorld->addRigidBody(body);



}
Пример #24
0
/////////////////////////////////////////
// UTILITY FUNCTIONS
/////////////
// returns TRUE if pt is inside of the CONVEX polygon LS, in world coordinates
bool cSpace::isPointInsidePoly(btVector3 pt,QList<btVector3> ls)
{
	int i,iplus;
	double z;
	btVector3 p1, p2;
	
	// traveling around the polygon point list in a CCW (right hand rule) direction 
	// if z is negative for all sides, pt is inside of convex polygon ls
	for(i=0; i<ls.size(); i++)
	{
		if(i == ls.size()-1) iplus = 0;
		else iplus = i+1;
		
		p1 = ls[i];
		p2 = ls[iplus];
		
		// pt is on the right side z is positive, left is negative
		z = (pt.x() - p1.x())*(p2.y() - p1.y()) - (pt.y() - p1.y())*(p2.x() - p1.x());
		
		if(z > 0) return false; // if the point is to the right of any side pt is outside the polygon
	}
 	return true;
}
void TrackDisplay::createRectangle(Ogre::Vector3 pos, Ogre::Quaternion orient,
		Ogre::Vector3 scale, double w, double h, btVector3 color, std::vector<
				ogre_tools::BillboardLine*> &vec) {

	ogre_tools::BillboardLine* lines = newBillboardLine();

	lines->setPosition(pos);
	lines->setOrientation(orient);
	lines->setScale(scale);
	lines->setColor(color.x(), color.y(), color.z(), alpha_);

	lines->clear();
	lines->setLineWidth(lineWidth_);
	lines->setMaxPointsPerLine(5);
	lines->setNumLines(1);

	Ogre::Vector3 v;

	v = Ogre::Vector3(0, 0, 0);
	rviz::robotToOgre(v);
	lines->addPoint(v);
	v = Ogre::Vector3(w, 0, 0);
	rviz::robotToOgre(v);
	lines->addPoint(v);
	v = Ogre::Vector3(w, h, 0);
	rviz::robotToOgre(v);
	lines->addPoint(v);
	v = Ogre::Vector3(0, h, 0);
	rviz::robotToOgre(v);
	lines->addPoint(v);
	v = Ogre::Vector3(0, 0, 0);
	rviz::robotToOgre(v);
	lines->addPoint(v);

	vec.push_back(lines);
}
Пример #26
0
void BtDebugDraw::drawLine( const btVector3 &fromBt, const btVector3 &toBt, const btVector3 &color )
{
    Point3F from = btCast<Point3F>( fromBt );
    Point3F to = btCast<Point3F>( toBt );

    // Cull first if we have a frustum.
    //F32 distSquared = ( mCuller->getPosition() - from ).lenSquared();
    //if ( mCuller && distSquared > ( 150 * 150 ) ) //!mCuller->clipSegment( from, to ) )
    //return;

    // Do we need to flush the builder?
    if ( mVertexCount + 2 >= 1000 )
        flush();

    // Are we starting a new primitive?
    if ( mVertexCount == 0 )
        PrimBuild::begin( GFXLineList, 1000 );

    PrimBuild::color3f( color.x(), color.y(), color.z() );
    PrimBuild::vertex3f( from.x, from.y, from.z );
    PrimBuild::vertex3f( to.x, to.y, to.z );

    mVertexCount += 2;
}
Пример #27
0
void PhysicsWorld::drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
{
    if (debugRenderer_)
        debugRenderer_->AddLine(ToVector3(from), ToVector3(to), Color(color.x(), color.y(), color.z()), debugDepthTest_);
}
Пример #28
0
void HfFluidDemo_GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int	debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax)
{


	glPushMatrix(); 
	btglMultMatrix(m);

	if (shape->getShapeType() == UNIFORM_SCALING_SHAPE_PROXYTYPE)
	{
		const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape);
		const btConvexShape* convexShape = scalingShape->getChildShape();
		float	scalingFactor = (float)scalingShape->getUniformScalingFactor();
		{
			btScalar tmpScaling[4][4]={{scalingFactor,0,0,0},
			{0,scalingFactor,0,0},
			{0,0,scalingFactor,0},
			{0,0,0,1}};

			drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode,worldBoundsMin,worldBoundsMax);
		}
		glPopMatrix();
		return;
	}

	if (shape->getShapeType() == HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE)
	{
		btConvexShape* convexShape = ((btHfFluidBuoyantConvexShape*)shape)->getConvexShape();
		btTransform I;
		I.setIdentity();
		btScalar mat[16];
		I.getOpenGLMatrix (&mat[0]);
		drawOpenGL (mat, convexShape, color, debugMode, worldBoundsMin, worldBoundsMax);
		return;
	}

	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
	{
		const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
		for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
		{
			btTransform childTrans = compoundShape->getChildTransform(i);
			const btCollisionShape* colShape = compoundShape->getChildShape(i);
			btScalar childMat[16];
			childTrans.getOpenGLMatrix(childMat);
			drawOpenGL(childMat,colShape,color,debugMode,worldBoundsMin,worldBoundsMax);

		}

	} else
	{
		if(m_textureenabled&&(!m_textureinitialized))
		{
			GLubyte*	image=new GLubyte[256*256*3];
			for(int y=0;y<256;++y)
			{
				const int	t=y>>4;
				GLubyte*	pi=image+y*256*3;
				for(int x=0;x<256;++x)
				{
					const int		s=x>>4;
					const GLubyte	b=180;					
					GLubyte			c=b+((s+t&1)&1)*(255-b);
					pi[0]=pi[1]=pi[2]=c;pi+=3;
				}
			}
			glGenTextures(1,(GLuint*)&m_texturehandle);
			glBindTexture(GL_TEXTURE_2D,m_texturehandle);
			glTexEnvf(GL_TEXTURE_ENV,GL_TEXTURE_ENV_MODE,GL_MODULATE);
			glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
			glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR);
			glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
			glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
			gluBuild2DMipmaps(GL_TEXTURE_2D,3,256,256,GL_RGB,GL_UNSIGNED_BYTE,image);
			delete[] image;

			glMatrixMode(GL_TEXTURE);
			glLoadIdentity();
			glScalef(0.025,0.025,0.025);

			static const GLfloat	planex[]={1,0,0,0};
			static const GLfloat	planey[]={0,1,0,0};
			static const GLfloat	planez[]={0,0,1,0};
			glTexGenfv(GL_S,GL_OBJECT_PLANE,planex);
			glTexGenfv(GL_T,GL_OBJECT_PLANE,planez);
			glTexGeni(GL_S,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR);
			glTexGeni(GL_T,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR);
			glEnable(GL_TEXTURE_GEN_S);
			glEnable(GL_TEXTURE_GEN_T);
			glEnable(GL_TEXTURE_GEN_R);
			m_textureinitialized=true;
		}
		//drawCoordSystem();

		//glPushMatrix();
		glEnable(GL_COLOR_MATERIAL);
		if(m_textureenabled) 
		{
			glEnable(GL_TEXTURE_2D);
			glBindTexture(GL_TEXTURE_2D,m_texturehandle);
		} else
		{
			glDisable(GL_TEXTURE_2D);
		}


		glColor3f(color.x(),color.y(), color.z());		

		bool useWireframeFallback = true;

		if (!(debugMode & btIDebugDraw::DBG_DrawWireframe))
		{
Пример #29
0
static void RenderCallback()
{
	// Clear buffers
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    
	// Setup camera
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0f, ((float)glutGet(GLUT_WINDOW_WIDTH))/((float)glutGet(GLUT_WINDOW_HEIGHT)), 1.0f, 10000.0f);
	gluLookAt(Eye.x(), Eye.y(), Eye.z(), Eye.x() + Dir.x(), Eye.y() + Dir.y(), Eye.z() + Dir.z(), 0.0f, 1.0f, 0.0f);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glEnable(GL_LIGHTING);

	//clear previous frames result
	gNormal.setValue(10,0,0);
	gPoint.setValue(0,0,0);
	gDepth = 999.999;
	gLastUsedMethod = -1;
	gNumGjkIterations = -1;


	TestEPA(gConvex0, gConvex1);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	btVector3 RefSep(btScalar(0.), btScalar(0.), btScalar(0.));
	float RefDMin=0.f;
	bool RefResult = false;
	if(gRefMode)
		RefResult = ReferenceCode(gConvex0, gConvex1, RefDMin, RefSep);

//	DrawLine(gPoint, gPoint + gNormal*20.0f, btVector3(1,0,0), 2.0f);
//	printf("%f:  %f  %f  %f\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());

#ifdef VERBOSE_TEXT_ONSCREEN
	glColor3f(255.f, 255.f, 255.f);

	setOrthographicProjection();
	float xOffset = 10.f;
	float yStart = 20.f;
	float yIncr = 20.f;
	char buf[124];

	sprintf(buf,"gDepth=%f:  gNormal=(%f  %f  %f)\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
	GLDebugDrawString(xOffset,yStart,buf);
	yStart += yIncr;

	sprintf(buf,"num GJK iterations =%d\n", gNumGjkIterations);
	GLDebugDrawString(xOffset,yStart,buf);
	yStart += yIncr;

	sprintf(buf,"gLastUsedMethod=%d\n", gLastUsedMethod);
	GLDebugDrawString(xOffset,yStart,buf);
	yStart += yIncr;

	

	

	if (gLastUsedMethod >= 3)
	{
		switch (	gMethod)
		{
		case 0:
			sprintf(buf,"Bullet GjkEpa Penetration depth solver (zlib free\n" );
			break;
		case 1:
			sprintf(buf,"Bullet Minkowski sampling Penetration depth solver\n" );
			break;
		case 2:
				sprintf(buf,"Solid35 EPA Penetration depth solver\n" );
				break;
		case 3:
			sprintf(buf,"EPA Penetration depth solver (Experimental/WorkInProgress, zlib free\n" );
			break;
		default:
				sprintf(buf,"Unknown Penetration Depth\n" );
		}
		GLDebugDrawString(xOffset,yStart,buf);
		yStart += yIncr;

	} else
	{
		sprintf(buf,"Hybrid GJK method %d\n", gLastUsedMethod);
		GLDebugDrawString(xOffset,yStart,buf);
		yStart += yIncr;
	}

	if (gLastDegenerateSimplex)
	{
		sprintf(buf,"DegenerateSimplex %d\n", gLastDegenerateSimplex);
		GLDebugDrawString(xOffset,yStart,buf);
		yStart += yIncr;
	}

	


	resetPerspectiveProjection();
#endif //VERBOSE_TEXT_ONSCREEN

	btVector3 color(0,0,0);
	gConvex0.Render(false, color);
	gConvex1.Render(false, color);

	if(gDepth<0.0f)
	{
		btTransform Saved = gConvex0.mTransform;
		gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() - btVector3(gNormal*gDepth));
		gConvex0.Render(true, btVector3(1.0f, 0.5f, 0.0f));
		gConvex0.mTransform = Saved;
	}
	else
	{
		DrawLine(gPoint, gPoint + gNormal, btVector3(0,1,0), 2.0f);
	}

	if(RefResult & gRefMode)
	{
		btTransform Saved = gConvex0.mTransform;
		gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(RefSep*RefDMin));
		gConvex0.Render(true, btVector3(0.0f, 0.5f, 1.0f));
		gConvex0.mTransform = Saved;
	}

	glutSwapBuffers();	
}
Пример #30
0
inline bool IsAlmostZero(const btVector3& v)
{
	if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6)	return false;
	return true;
}