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

void SpuContactResult::addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)
{
#ifdef DEBUG_SPU_COLLISION_DETECTION
	spu_printf("*** SpuContactResult::addContactPoint: depth = %f\n",depth);
	spu_printf("*** normal = %f,%f,%f\n",normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
	spu_printf("*** position = %f,%f,%f\n",pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());
#endif //DEBUG_SPU_COLLISION_DETECTION
	

#ifdef DEBUG_SPU_COLLISION_DETECTION
 //   int sman = sizeof(rage::phManifold);
//	spu_printf("sizeof_manifold = %i\n",sman);
#endif //DEBUG_SPU_COLLISION_DETECTION

	btPersistentManifold* localManifold = m_spuManifold;

	btVector3	normalB(normalOnBInWorld.getX(),normalOnBInWorld.getY(),normalOnBInWorld.getZ());
	btVector3	pointWrld(pointInWorld.getX(),pointInWorld.getY(),pointInWorld.getZ());

	//process the contact point
	const bool retVal = ManifoldResultAddContactPoint(normalB,
		pointWrld,
		depth,
		localManifold,
		m_rootWorldTransform0,
		m_rootWorldTransform1,
		m_combinedFriction,
		m_combinedRestitution,
		m_isSwapped);
	m_RequiresWriteBack = m_RequiresWriteBack || retVal;
}

void GLDebugDrawer::drawSphere (const btVector3 &p, btScalar radius, const btVector3 &color) {

	glColor4f(color.getX(), color.getY(), color.getZ(), btScalar(1.0f));
	glPushMatrix();
	glTranslatef(p.getX(), p.getY(), p.getZ());

	int lats = 5;
	int longs = 5;

	int i, j;
	for (i = 0; i <= lats; i++) {
		btScalar lat0 = SIMD_PI * (-btScalar(0.5) + (btScalar) (i - 1) / lats);
		btScalar z0  = radius * sin(lat0);
		btScalar zr0 =  radius * cos(lat0);

		btScalar lat1 = SIMD_PI * (-btScalar(0.5) + (btScalar) i / lats);
		btScalar z1 = radius * sin(lat1);
		btScalar zr1 = radius * cos(lat1);

		Mygl gl(longs * 2, 0, false, false, true);
		gl.glBegin(GL_TRIANGLE_STRIP);	// GL_QUAD_STRIP
		for (j = 0; j <= longs; j++) {
			btScalar lng = 2 * SIMD_PI * (btScalar) (j - 1) / longs;
			btScalar x = cos(lng);
			btScalar y = sin(lng);

			gl.glNormal(x * zr0, y * zr0, z0);	gl.glVertex(x * zr0, y * zr0, z0);
			gl.glNormal(x * zr1, y * zr1, z1);	gl.glVertex(x * zr1, y * zr1, z1);
		}
		gl.glEnd();
	}

	glPopMatrix();
}

void BulletDebug::drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
{
	_dd.drawLine(
		Ogre::Vector3(from.getX(), from.getY(), from.getZ()),
		Ogre::Vector3(to.getX(), to.getY(), to.getZ()),
		Ogre::ColourValue(color.getX(), color.getY(), color.getZ()));
}

btVector3 Leaf::getFlutter(const btVector3& angularPos, float effectiveArea)
{
	btVector3 angularPos2 = btVector3(angularPos.getX() / (float)glfwGetTime(), angularPos.getY() / (float)glfwGetTime(), angularPos.getZ() / (float)glfwGetTime());
	
	const float width = 0.005;
	const float length = 0.005;

	float farokskonstant = sqrt(mass / (density*(pow(length, 2)*width)));

	float flutterX = (-effectiveArea*sinf(angularPos2.getX())*sinf(angularPos2.getX()-3.14) +
		(1 - effectiveArea)*abs(cosf(angularPos2.getX()))*cosf(angularPos2.getX() - 3.14 / 2) -
		abs(sinf(angularPos2.getX() + 3.14))*cosf(angularPos2.getX() / 2 + 3.14 / 2)) / (farokskonstant);

	float flutterY = (-effectiveArea*pow(leafBody->getAngularVelocity().getY(), 2) / farokskonstant * 
		(sinf(angularPos2.getY())*cosf(angularPos2.getY()-3.14) + (cosf(angularPos2.getY()))*sinf(angularPos2.getY() - 3.14) 
		- abs(sinf(angularPos2.getY() + 3.14))*sinf(angularPos.getY() / 2 + 3.14 / 2)) / (farokskonstant))-1/farokskonstant;
	
	float flutterZ = ((1 - effectiveArea )- sinf(angularPos2.getZ())*sinf(angularPos2.getZ()-3.14) + 
		effectiveArea* abs(cosf(angularPos2.getZ()))*cosf(angularPos2.getZ() - 3.14) - abs(sinf(angularPos2.getZ()))
		*cosf(angularPos2.getZ() / 2 + 3.14 / 2)) / (farokskonstant);

	//std::cout << flutter << '\n';*/
	flutter = btVector3(flutterX, flutterY, flutterZ);
	return flutter;
}

// =============================================
void DebugDrawer::drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
{
	line.origin.Set(from.getX(), from.getY(), from.getZ());
	line.destination.Set(to.getX(), to.getY(), to.getZ());
	line.color.Set(color.getX(), color.getY(), color.getZ());
	line.Render();
}

bool boxIsContainedWithinBox(const btVector3& amin,const btVector3& amax,const btVector3& bmin,const btVector3& bmax)
{
return
amin.getX() >= bmin.getX() && amax.getX() <= bmax.getX() &&
amin.getY() >= bmin.getY() && amax.getY() <= bmax.getY() &&
amin.getZ() >= bmin.getZ() && amax.getZ() <= bmax.getZ();
}

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),
#ifdef _BT_USE_CENTER_LIMIT_
									 m_limit(),
#endif
									 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_normalCFM(0),
									 m_normalERP(0),
									 m_stopCFM(0),
									 m_stopERP(0)
{
	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() );
	
#ifndef	_BT_USE_CENTER_LIMIT_
	//start with free
	m_lowerLimit = btScalar(1.0f);
	m_upperLimit = btScalar(-1.0f);
	m_biasFactor = 0.3f;
	m_relaxationFactor = 1.0f;
	m_limitSoftness = 0.9f;
	m_solveLimit = false;
#endif
	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}

void GLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor) {
	glBegin(GL_LINES);
		glColor3f(fromColor.getX(), fromColor.getY(), fromColor.getZ());
		glVertex3d(from.getX(), from.getY(), from.getZ());
		glColor3f(toColor.getX(), toColor.getY(), toColor.getZ());
		glVertex3d(to.getX(), to.getY(), to.getZ());
	glEnd();
}

void DebugDrawRouter::drawLine( const btVector3& from,const btVector3& to, const btVector3& color )
{
    COCA_ASSERT( _drawer );

    _drawer->drawLine( from.getX(), from.getY(), from.getZ(), to.getX(), to.getY(), to.getZ(),
                       static_cast<int>( color.getX() ), static_cast<int>( color.getY() ),
                       static_cast<int>( color.getZ() ) );
}

	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
	{
		glBegin(GL_LINES);
			glColor3f(color.getX(), color.getY(), color.getZ());
			glVertex3d(from.getX(), from.getY(), from.getZ());
			glVertex3d(to.getX(), to.getY(), to.getZ());
		glEnd();
	}

void ShapeDrawer::drawAngularLimit(const btTransform& space, const btVector3& angularLower, const btVector3& angularUpper)
{
    glPushMatrix();
    btScalar m[16];
    
    m[0] = 1.0; m[1] = m[2] = m[3] = 0.0;
    m[4] = 0.0; m[5] = 1.0; m[6] = m[7] = 0.0;
    m[8] = m[9] = 0.0; m[10] = 1.0; m[11] = 0.0;
    m[12] = m[13] = m[14] = 0.0; m[15] = 1.0;
    space.getOpenGLMatrix(m);
    glMultMatrixf((const GLfloat*)m);
    float x, y, z;
    glBegin( GL_LINES );
	
    if(angularLower.getX() < angularUpper.getX()) {
        glColor3f(1.f, 0.f, 0.f);
        glVertex3f(0.f, 0.f, 0.f);
        
        y = sin((float)angularLower.getX());
        z = cos((float)angularLower.getX());
        glVertex3f(0.f, y, z);
        
        glVertex3f(0.f, 0.f, 0.f);
        
        y = sin((float)angularUpper.getX());
        z = cos((float)angularUpper.getX());
        glVertex3f(0.f, y, z);
    }
	
    if(angularLower.getY() < angularUpper.getY()) {
        glColor3f(0.f, 1.f, 0.f);					
        glVertex3f(0.f, 0.f, 0.f);
        x = cos((float)angularLower.getY());
        z = sin((float)angularLower.getY());
        glVertex3f(x, 0.f, z);
        
        glVertex3f(0.f, 0.f, 0.f);
        x = cos((float)angularUpper.getY());
        z = sin((float)angularUpper.getY());
        glVertex3f(x, 0.f, z);
	}
	
	if(angularLower.getZ() < angularUpper.getZ()) {
        glColor3f(0.f, 0.f, 1.f);					
        glVertex3f(0.f, 0.f, 0.f);
        x = cos((float)angularLower.getZ());
        y = sin((float)angularLower.getZ());	
        glVertex3f(x, y, 0.f);
        
        glVertex3f(0.f, 0.f, 0.f);
        x = cos((float)angularUpper.getZ());
        y = sin((float)angularUpper.getZ());	
        glVertex3f(x, y, 0.f);
	}
	
	glEnd();
    glPopMatrix();
}

/* ----------------------------------------------------------------------- 
 | build bullet box shape
 | 
 | : default create 125 (5x5x5) dynamic object
   ----------------------------------------------------------------------- */
bool
buildBoxShapeArray(Ogre::SceneManager* sceneMgr, btDynamicsWorld* dynamicsWorld, btAlignedObjectArray<btCollisionShape*>& collisionShapes,
                   const btVector3& array_size, btScalar scale)
{
    btTransform startTransform;
    startTransform.setIdentity();

    btScalar mass(1.f);	
    btVector3 localInertia(0,0,0);	
    btBoxShape* colShape = new btBoxShape(btVector3(scale, scale, scale));
    btAssert(colShape);    
    colShape->calculateLocalInertia(mass,localInertia);
    collisionShapes.push_back(colShape);

    float start_x = - array_size.getX()/2;
    float start_y = array_size.getY();
    float start_z = - array_size.getZ()/2;

    int index = 0;
    for (int k=0;k<array_size.getY();k++)
    {
	    for (int i=0;i<array_size.getX();i++)
	    {
            for(int j=0;j<array_size.getZ();j++)
		    {
			    startTransform.setOrigin(scale * btVector3(
								    btScalar(2.0*i + start_x),
								    btScalar(20+2.0*k + start_y),
								    btScalar(2.0*j + start_z)));

			    //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);
                body->setContactProcessingThreshold(BT_LARGE_FLOAT);

                if (sceneMgr)
                {
                    Ogre::Entity* ent = sceneMgr->createEntity("ent_" + Ogre::StringConverter::toString(index++),"Barrel.mesh");

                    Ogre::SceneNode* node = sceneMgr->getRootSceneNode()->createChildSceneNode("node_box_" + Ogre::StringConverter::toString(index++));
                    node->attachObject(ent);
                    node->setPosition(startTransform.getOrigin().getX(), startTransform.getOrigin().getY(), startTransform.getOrigin().getZ());

                    const Ogre::AxisAlignedBox& aabb = ent->getBoundingBox();
                    const Ogre::Vector3& boxScale = (aabb.getMaximum() - aabb.getMinimum())/2.0f;
                    node->scale(scale/boxScale.x, scale/boxScale.y, scale/boxScale.z);

                    body->setUserPointer((void*)node);
                }

			    dynamicsWorld->addRigidBody(body);
		    }
	    }		
    }

    return true;
}

IMesh* MeshTools::createMeshFromSoftBody(btSoftBody* softBody)
{
	SMeshBuffer* buffer = new SMeshBuffer();
	video::S3DVertex vtx;
	vtx.Color.set(255,255,255,255);

    /* Each soft body contain an array of vertices (nodes/particles_mass)   */
    btSoftBody::tNodeArray&   nodes(softBody->m_nodes);

    // Convert bullet nodes to vertices
    for(int i=0;i<nodes.size();++i)
    {
        const btSoftBody::Node&	n=nodes[i];
        const btVector3 normal=n.m_n;
        const btVector3 pos=n.m_x;

        vtx.Pos.set(pos.getX(), pos.getY(), pos.getZ());
		vtx.Normal.set(normal.getX(), normal.getY(), normal.getZ());

        // TODO: calculate texture coords
        //vtx.TCoords.set(tsx, tsy);

        buffer->Vertices.push_back(vtx);
    }

    // Convert triangles of the softbody to an index list
    for(int i=0;i<softBody->m_faces.size();++i)
    {
        auto	faces = softBody->m_faces;

        btSoftBody::Node*   node_0=faces[i].m_n[0];
        btSoftBody::Node*   node_1=faces[i].m_n[1];
        btSoftBody::Node*   node_2=faces[i].m_n[2];

        const int indices[] =
        {
            int(node_0-&nodes[0]),
            int(node_1-&nodes[0]),
            int(node_2-&nodes[0])
        };

        for(int j=0;j<3;++j)
            buffer->Indices.push_back(indices[j]);
    }

	buffer->recalculateBoundingBox();

	// Default the mesh to stream because most likely we will be updating
	// the vertex positions every frame to deal with softbody movement.
	buffer->setHardwareMappingHint(EHM_STREAM);

	SMesh* mesh = new SMesh();
	mesh->addMeshBuffer(buffer);
	mesh->recalculateBoundingBox();
	buffer->drop();
	return mesh;
}

/// conservative test for overlap between two aabbs
static bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
								const btVector3 &aabbMin2, const btVector3 &aabbMax2)
{
	bool overlap = true;
	overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
	overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
	overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
	return overlap;
}

    void PhysicsDebugDraw::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor)
    {
		debugRenderer->drawLine(
			Vec3(from.getX(), from.getY(), from.getZ()),
			Vec3(to.getX(), to.getY(), to.getZ()),
			Vec4(fromColor.getX(), fromColor.getY(), fromColor.getZ(), 1.f),
			Vec4(toColor.getX(), toColor.getY(), toColor.getZ(), 1.f)
		);
    }