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

btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
									 btVector3& axisInA,btVector3& axisInB)
									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
									 m_angularOnly(false),
									 m_enableAngularMotor(false)
{
	m_rbAFrame.getOrigin() = pivotInA;
	
	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);

	btVector3 rbAxisA2;
	btScalar projection = axisInA.dot(rbAxisA1);
	if (projection >= 1.0f - SIMD_EPSILON) {
		rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
	} else if (projection <= -1.0f + SIMD_EPSILON) {
		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
	} else {
		rbAxisA2 = axisInA.cross(rbAxisA1);
		rbAxisA1 = rbAxisA2.cross(axisInA);
	}

	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
	btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);	
	
	m_rbBFrame.getOrigin() = pivotInB;
	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
									rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
									rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
	
	//start with free
	m_lowerLimit = btScalar(1e30);
	m_upperLimit = btScalar(-1e30);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;

}

 void	drawLine(const btVector3& from,const btVector3& to,const btVector3& fromcolor, const btVector3& tocolor ) {
     v[0] = from.getX();
     v[1] = from.getY();
     v[2] = from.getZ();
     v[3] = to.getX();
     v[4] = to.getY();
     v[5] = to.getZ();
     c[0]  = fromcolor.getX();
     c[1]  = fromcolor.getY();
     c[2]  = fromcolor.getZ();
     c[3]  = tocolor.getX();
     c[4]  = tocolor.getY();
     c[5]  = tocolor.getZ();
     glEnableClientState(GL_VERTEX_ARRAY);
     glVertexPointer(3, GL_FLOAT, 0, v);
     glDrawArrays(GL_LINES, 0, 2);
 }

// -----------------------------------------------------------------------------
void IrrDebugDrawer::drawLine(const btVector3& from, const btVector3& to,
                              const btVector3& color)
{
    video::SColor c(255, (int)(color.getX()*255), (int)(color.getY()*255),
                         (int)(color.getZ()*255)                          );
    irr_driver->getVideoDriver()->draw3DLine((const core::vector3df&)from,
                                             (const core::vector3df&)to, c);
}

void OgreDebugDrawer::drawLine( const btVector3 &from, const btVector3 &to, const btVector3 &color )
{
    ColourValue c( color.getX(), color.getY(), color.getZ() );  
    c.saturate();
    mLines->position( cvt(from) );
    mLines->colour( c );
    mLines->position( cvt(to) );
    mLines->colour( c );
}

    void
    PhysicsDebugDrawer::drawLine
    (const btVector3& from,const btVector3& to,const btVector3& color, const btVector3& color2)
    {
        // Too many lines to be effective?
        /*
        BoundingBox bb;
        bb.minimum.x = std::min(from.x(),to.x());
        bb.minimum.y = std::min(from.y(),to.y());
        bb.minimum.z = std::min(from.z(),to.z());

        bb.maximum.x = std::max(from.x(),to.x());
        bb.maximum.y = std::max(from.y(),to.y());
        bb.maximum.z = std::max(from.z(),to.z());

        if (mCamera->inFrustum(bb))
        {
        */
            PhysicsDebugVertex a, b;

            a.Position.x = from.getX();
            a.Position.y = from.getY();
            a.Position.z = from.getZ();
            a.Color.r = color.getX();
            a.Color.g = color.getY();
            a.Color.b = color.getZ();
            mVertexBuffer.push_back(a);

            b.Position.x = to.getX();
            b.Position.y = to.getY();
            b.Position.z = to.getZ();
            b.Color.r = color2.getX();
            b.Color.g = color2.getY();
            b.Color.b = color2.getZ();
            mVertexBuffer.push_back(b);
            /*
        }
        else
        {
            auto log = getLog();
            log->error("Skipping debug point");
        }
        */
    }

// -----------------------------------------------------------------------------
void IrrDebugDrawer::drawLine(const btVector3& from, const btVector3& to,
                              const btVector3& color)
{
    video::SColor c(255, (int)(color.getX()*255), (int)(color.getY()*255),
                         (int)(color.getZ()*255)                          );

    //World::getWorld()->getCa

    if (from.distance2(m_camera_pos) > 10000) return;

    std::vector<float>& v = m_lines[c];
    v.push_back(from.getX());
    v.push_back(from.getY());
    v.push_back(from.getZ());
    v.push_back(to.getX());
    v.push_back(to.getY());
    v.push_back(to.getZ());
    //draw3DLine((const core::vector3df&)from, (const core::vector3df&)to, c);
}

btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
									 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
									 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
									 m_angularOnly(false),
									 m_enableAngularMotor(false),
									 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
									 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
									 m_useReferenceFrameA(useReferenceFrameA),
									 m_flags(0),m_limit()
{
	m_rbAFrame.getOrigin() = pivotInA;
	
	// since no frame is given, assume this to be zero angle and just pick rb transform axis
	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);

	btVector3 rbAxisA2;
	btScalar projection = axisInA.dot(rbAxisA1);
	if (projection >= 1.0f - SIMD_EPSILON) {
		rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
	} else if (projection <= -1.0f + SIMD_EPSILON) {
		rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
		rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
	} else {
		rbAxisA2 = axisInA.cross(rbAxisA1);
		rbAxisA1 = rbAxisA2.cross(axisInA);
	}

	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
									rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
									rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );

	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
	btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
	btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);	
	
	m_rbBFrame.getOrigin() = pivotInB;
	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
									rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
									rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
	
	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}

void OgreBtDebugDrawer::drawLine(const btVector3 &from, const btVector3 &to,
		const btVector3 &color) {
	if (color == btVector3(0, 0, 0)) {
		drawLine(OgreBulletUtils::convert(from), OgreBulletUtils::convert(to),
				Ogre::ColourValue(1, 1, 1));
	} else {
		drawLine(OgreBulletUtils::convert(from), OgreBulletUtils::convert(to),
				Ogre::ColourValue(color.getX(), color.getY(), color.getZ()));
	}
}

/** Interpolates the normal at the given position for the triangle with
 *  a given index. The position must be inside of the given triangle.
 *  \param index Index of the triangle to use.
 *  \param position The position for which to interpolate the normal.
 */
btVector3 TriangleMesh::getInterpolatedNormal(unsigned int index,
                                              const btVector3 &position) const
{
    btVector3 p1, p2, p3;
    getTriangle(index, &p1, &p2, &p3);
    btVector3 n1, n2, n3;
    getNormals(index, &n1, &n2, &n3);

    // Compute the Barycentric coordinates of position inside  triangle 
    // p1, p2, p3.
    btVector3 edge1 = p2 - p1;
    btVector3 edge2 = p3 - p1;

    // Area of triangle ABC
    btScalar p1p2p3 = edge1.cross(edge2).length2();

    // Area of BCP
    btScalar p2p3p = (p3 - p2).cross(position - p2).length2();

    // Area of CAP
    btScalar p3p1p = edge2.cross(position - p3).length2();
    btScalar s = btSqrt(p2p3p / p1p2p3);
    btScalar t = btSqrt(p3p1p / p1p2p3);
    btScalar w = 1.0f - s - t;

#ifdef NORMAL_DEBUGGING
    btVector3 regen_position = s * p1 + t * p2 + w * p3;

    if((regen_position - position).length2() >= 0.0001f)
    {
        printf("bary:\n");
        printf("new: %f %f %f\n", regen_position.getX(),regen_position.getY(),regen_position.getZ());
        printf("old: %f %f %f\n", position.getX(), position.getY(),position.getZ());
        printf("stw: %f %f %f\n", s, t, w);
        printf("p1:  %f %f %f\n", p1.getX(),p1.getY(),p1.getZ());
        printf("p2:  %f %f %f\n", p2.getX(),p2.getY(),p2.getZ());
        printf("p3:  %f %f %f\n", p3.getX(),p3.getY(),p3.getZ());
        printf("pos: %f %f %f\n", position.getX(),position.getY(),position.getZ());
    }
#endif

    return s*n1 + t*n2 + w*n3;
}   // getInterpolatedNormal

/*
 * hitFruit
 * aktualisiert comboCount und Punkte und spielt sounds ab
 * @param pos Position der Frucht
 */
void GameModeTraining::hitFruit(btVector3 pos) {

	playSound(hit_fruit, pos.getX(), pos.getZ());

	if(gamestate->getStatus() == ACTIVE) {
		this->comboTime = 1.0;
		++(this->comboCount);
		this->gamestate->changePoints(1);
	}
}

bool 
CGrassSticks::buildGrassSticks(Ogre::SceneManager* sceneMgr, btDynamicsWorld* dynamicsWorld, btSoftBodyWorldInfo &softBodyWorldInfo)
{    
    // create our grass mesh, and create a grass entity from it
    if (!sceneMgr->hasEntity(GRASS_MESH_NAME))
    {
	    createGrassMesh();
    } // End if

    const int		n=16;
	const int		sg=4;    
	const btScalar	sz=16;
	const btScalar	hg=4;
	const btScalar	in=1/(btScalar)(n-1);
    int             index = 0;

	for(int y=0;y<n;++y)
	{
		for(int x=0;x<n;++x)
		{
			const btVector3	org(-sz+sz*2*x*in,
				1,
				-sz+sz*2*y*in);
			btSoftBody*		psb=btSoftBodyHelpers::CreateRope(softBodyWorldInfo, org,
				org+btVector3(hg*0.001f,hg,0),
				sg,
				1);
			psb->m_cfg.kDP		=	0.005f;
			psb->m_cfg.kCHR		=	0.1f;
			for(int i=0;i<3;++i)
			{
				psb->generateBendingConstraints(2+i);
			}
			psb->setMass(1,0);
			psb->setTotalMass(0.01f);
                       
            static_cast<btSoftRigidDynamicsWorld*>(dynamicsWorld)->addSoftBody(psb);
            
            const Ogre::String& strIndex = Ogre::StringConverter::toString(index++);
            Ogre::Entity* grass = sceneMgr->createEntity("Grass" + strIndex, GRASS_MESH_NAME);
            Ogre::SceneNode* node = sceneMgr->getRootSceneNode()->createChildSceneNode("node_grass_" + strIndex
                ,Ogre::Vector3(org.getX(), org.getY(), org.getZ())
                ,Ogre::Quaternion(Ogre::Degree(0), Vector3::UNIT_Y));
            node->attachObject(grass);
            node->scale(1.0f, Ogre::Math::RangeRandom(0.85f, 1.15f), 1.0f);
            node->setVisible(true);
                                    
            psb->setUserPointer((void*)(grass->getSubEntity(0)));
		} // End for
	} // End for

    dynamicsWorld->setInternalTickCallback(&CGrassSticks::simulationTickCallback);
    
    return true;
}

void OgreDebugDrawer::drawTriangle( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2, const btVector3 &color, btScalar alpha )
{
    ColourValue c( color.getX(), color.getY(), color.getZ(), alpha );  
    c.saturate();
    mTriangles->position( cvt(v0) );
    mTriangles->colour( c );
    mTriangles->position( cvt(v1) );
    mTriangles->colour( c );
    mTriangles->position( cvt(v2) );
    mTriangles->colour( c );
}

void btRigidBody::setGravity(const btVector3& acceleration) 
{
    btAssert(!std::isnan(acceleration.getX()));
    btAssert(!std::isnan(acceleration.getY()));
    btAssert(!std::isnan(acceleration.getZ()));
	if (m_inverseMass != btScalar(0.0))
	{
		m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
	}
	m_gravity_acceleration = acceleration;
}

	void	SingularityDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor) {

		btDebugVertex l0, l1;
		l0.r = fromColor.getX();
		l0.g = fromColor.getY();
		l0.b = fromColor.getZ();
		l0.x = from.getX();
		l0.y = from.getY();
		l0.z = from.getZ();

		l1.r = toColor.getX();
		l1.g = toColor.getY();
		l1.b = toColor.getZ();
		l1.x = to.getX();
		l1.y = to.getY();
		l1.z = to.getZ();

		m_lineVertexList.push_back(l0);
		m_lineVertexList.push_back(l1);

	}

btVector3 EntityPhysic::colVelocity(btVector3 velocity_)
{

	int MAX=120;
	int MIN=50;
	if(velocity_.length()>MAX)
	{
		float radio=velocity_.length()/MAX;
		velocity_=btVector3(velocity_.getX()/radio,
				velocity_.getY()/radio,
				velocity_.getZ()/radio);
	}
	if(velocity_.length()<MIN)
	{
		float radio=velocity_.length()/MIN;
		velocity_=btVector3(velocity_.getX()/radio,
				velocity_.getY()/radio,
				velocity_.getZ()/radio);
	}
	return velocity_;
}