C++ btVector3::y方法代码示例

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

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

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

// 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;
}

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

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

/**
* 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);
}

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

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

/**
* 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);
}

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

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