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

void	btCreateLookAt(const btVector3& eye, const btVector3& center,const btVector3& up, float result[16])
{
        btVector3 f = (center - eye).normalized();
        btVector3 u = up.normalized();
		btVector3 s = (f.cross(u)).normalized();
        u = s.cross(f);

		result[0*4+0] = s.x();
        result[1*4+0] = s.y();
        result[2*4+0] = s.z();
		result[3*4+0] = -s.dot(eye);

		result[0*4+1] = u.x();
		result[1*4+1] = u.y();
        result[2*4+1] = u.z();
		result[3*4+1] = -u.dot(eye);

		result[0*4+2] =-f.x();
        result[1*4+2] =-f.y();
		result[2*4+2] =-f.z();
		result[3*4+2] = f.dot(eye);

		result[0*4+3] = 0.f;
		result[1*4+3] = 0.f;
		result[2*4+3] = 0.f;
		result[3*4+3] = 1.f;
}

// static helper method
static btVector3 getNormalizedVector(const btVector3& v)
{
	btVector3 n = v.normalized();
	if (n.length() < SIMD_EPSILON) {
		n.setValue(0, 0, 0);
	}
	return n;
}

btStaticPlaneShape::btStaticPlaneShape(const btVector3& planeNormal,btScalar planeConstant)
: btConcaveShape (), m_planeNormal(planeNormal.normalized()),
m_planeConstant(planeConstant),
m_localScaling(btScalar(0.),btScalar(0.),btScalar(0.))
{
	m_shapeType = STATIC_PLANE_PROXYTYPE;
	//	btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) );
}

void btGeneric6DofSpringConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
{
	btVector3 zAxis = axis1.normalized();
	btVector3 yAxis = axis2.normalized();
	btVector3 xAxis = yAxis.cross(zAxis);  // we want right coordinate system

	btTransform frameInW;
	frameInW.setIdentity();
	frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
								 xAxis[1], yAxis[1], zAxis[1],
								 xAxis[2], yAxis[2], zAxis[2]);

	// now get constraint frame in local coordinate systems
	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;

	calculateTransforms();
}

// static helper method
static btVector3
getNormalizedVector(const btVector3& v)
{
	btVector3 n(0, 0, 0);

	if (v.length() > SIMD_EPSILON) {
		n = v.normalized();
	}
	return n;
}

PhysicsSystem::PhysicsSystem(Engine& engine, const float& gravity, const btVector3& rotation) :
	System(engine),
	collisionConfiguration(*new btDefaultCollisionConfiguration()),
	dispatcher(*new btCollisionDispatcher(&collisionConfiguration)),
	broadphase(*new btDbvtBroadphase()),
	solver(*new btSequentialImpulseConstraintSolver),
	dynamicsWorld(*new btDiscreteDynamicsWorld(&dispatcher, &broadphase, &solver, &collisionConfiguration)),
	gravity(gravity),
	rotation(rotation.normalized())
{
    dynamicsWorld.setGravity(gravity*rotation);
} //PhysicsSystem

bool BtPlayer::_sweep( btVector3 *inOutCurrPos, const btVector3 &disp, CollisionList *outCol )
{
   btTransform start( btTransform::getIdentity() );
	start.setOrigin ( *inOutCurrPos );

   btTransform end( btTransform::getIdentity() );
	end.setOrigin ( *inOutCurrPos + disp );

   BtPlayerSweepCallback callback( mGhostObject, disp.normalized() );
	callback.m_collisionFilterGroup = mGhostObject->getBroadphaseHandle()->m_collisionFilterGroup;
	callback.m_collisionFilterMask = mGhostObject->getBroadphaseHandle()->m_collisionFilterMask;
	
	mGhostObject->convexSweepTest( mColShape, start, end, callback, 0.0f );

	inOutCurrPos->setInterpolate3( start.getOrigin(), end.getOrigin(), callback.m_closestHitFraction );
   if ( callback.hasHit() )
   {
      if ( outCol )
      {
         Collision& col = outCol->increment();
         dMemset( &col, 0, sizeof( col ) );

         col.normal = btCast<Point3F>( callback.m_hitNormalWorld );
         col.object = PhysicsUserData::getObject( callback.m_hitCollisionObject->getUserPointer() );

         if (disp.z() < 0.0f)
         {
            // We're sweeping down as part of the stepping routine.    In this
            // case we want to have the collision normal only point in the opposite direction.
            // i.e. up  If we include the sideways part of the normal then the Player class
            // velocity calculations using this normal will affect the player's forwards
            // momentum.  This is especially noticable on stairs as the rounded bottom of
            // the capsule slides up the corner of a stair.
            col.normal.set(0.0f, 0.0f, 1.0f);
         }
      }

      return true;
   }

   return false;
}

void	btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut)
{

	btVector3 separatingNormal = separatingNormal1.normalized();
	const btVector3 c0 = transA * hullA.m_localCenter;
	const btVector3 c1 = transB * hullB.m_localCenter;
	const btVector3 DeltaC2 = c0 - c1;


	btScalar curMaxDist=maxDist;
	int closestFaceB=-1;
	btScalar dmax = -FLT_MAX;
	{
		for(int face=0;face<hullB.m_faces.size();face++)
		{
			const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
			const btVector3 WorldNormal = transB.getBasis() * Normal;
			btScalar d = WorldNormal.dot(separatingNormal);
			if (d > dmax)
			{
				dmax = d;
				closestFaceB = face;
			}
		}
	}
				btVertexArray worldVertsB1;
				{
					const btFace& polyB = hullB.m_faces[closestFaceB];
					const int numVertices = polyB.m_indices.size();
					for(int e0=0;e0<numVertices;e0++)
					{
						const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]];
						worldVertsB1.push_back(transB*b);
					}
				}

	
	if (closestFaceB>=0)
		clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut);

}

// static helper method
static btVector3
getNormalizedVector(const btVector3& v)
{
	/*btVector3 n(0, 0, 0);

	if (v.fuzzyZero()) return n;

	if (v.length() > SIMD_EPSILON) {
		n = v.normalized();
	}
	return n;*/
	btVector3 n;
	if (v.fuzzyZero()) {
		n.setValue(0, 0, 0);
	}
	else {
		n = v.normalized();
	}

	return n;
}

static inline btVector3	Vector3Rand()
{
	const btVector3	p=btVector3(SignedUnitRand(),SignedUnitRand(),SignedUnitRand());
	return(p.normalized());
}

    //-----------------------------------------------------------------------
    //                 s t e p F o r w a r d A n d S t r a f e
    //-----------------------------------------------------------------------
    void TKinematicCharacterTest::stepForwardAndStrafe (btCollisionWorld* collisionWorld, const btVector3& walkMove)
    {
        btVector3 originalDir = walkMove.normalized();
        if (walkMove.length() < SIMD_EPSILON)
        {
            originalDir.setValue(0.f,0.f,0.f);
        }
        //	printf("originalDir=%f,%f,%f\n",originalDir[0],originalDir[1],originalDir[2]);
        // phase 2: forward and strafe
        btTransform start, end;
        m_targetPosition = m_currentPosition + walkMove;
        start.setIdentity ();
        end.setIdentity ();

        btScalar fraction = 1.0;
        btScalar distance2 = (m_currentPosition-m_targetPosition).length2();
        //	printf("distance2=%f\n",distance2);

        if (m_touchingContact)
        {
            if (originalDir.dot(m_touchingNormal) > btScalar(0.0))
                updateTargetPositionBasedOnCollision (m_touchingNormal);
        }

        int maxIter = 10;

        while (fraction > btScalar(0.01) && maxIter-- > 0)
        {
            start.setOrigin (m_currentPosition);
            end.setOrigin (m_targetPosition);

            TKinematicClosestNotMeConvexResultCallback callback (m_ghostObject);
            callback.m_collisionFilterGroup = getGhostObject()->getBroadphaseHandle()->m_collisionFilterGroup;
            callback.m_collisionFilterMask = getGhostObject()->getBroadphaseHandle()->m_collisionFilterMask;


            btScalar margin = m_convexShape->getMargin();
            m_convexShape->setMargin(margin + m_addedMargin);


            if (m_useGhostObjectSweepTest)
            {
                m_ghostObject->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
            } else
            {
                collisionWorld->convexSweepTest (m_convexShape, start, end, callback, collisionWorld->getDispatchInfo().m_allowedCcdPenetration);
            }

            m_convexShape->setMargin(margin);


            fraction -= callback.m_closestHitFraction;

            if (callback.hasHit())
            {	
                if(callback.m_hitCollisionObject->isStaticObject())
                {
                    // we moved only a fraction
                    btScalar hitDistance = (callback.m_hitPointWorld - m_currentPosition).length();
                    if (hitDistance<0.f)
                    {
                        //				printf("neg dist?\n");
                    }

                    /* If the distance is farther than the collision margin, move */
                    if (hitDistance > m_addedMargin)
                    {
                        //				printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
                        m_currentPosition.setInterpolate3 (m_currentPosition, m_targetPosition, callback.m_closestHitFraction);
                    }

                    updateTargetPositionBasedOnCollision (callback.m_hitNormalWorld);
                    btVector3 currentDir = m_targetPosition - m_currentPosition;
                    distance2 = currentDir.length2();
                    if (distance2 > SIMD_EPSILON)
                    {
                        currentDir.normalize();
                        /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */
                        if (currentDir.dot(originalDir) <= btScalar(0.0))
                        {
                            break;
                        }
                    } else
                    {
                        //				printf("currentDir: don't normalize a zero vector\n");
                        break;
                    }
                }
            } else {
                // we moved whole way
                m_currentPosition = m_targetPosition;
            }

            //	if (callback.m_closestHitFraction == 0.f)
            //		break;

        }
//.........这里部分代码省略.........