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

// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
: btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
 m_anchor(anchor),
 m_axis1(axis1),
 m_axis2(axis2)
{
    // build frame basis
    // 6DOF constraint uses Euler angles and to define limits
    // it is assumed that rotational order is :
    // Z - first, allowed limits are (-PI,PI);
    // new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
    // used to prevent constraint from instability on poles;
    // new position of X, allowed limits are (-PI,PI);
    // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
    // Build the frame in world coordinate system first
    btVector3 zAxis = axis1.normalize();
    btVector3 yAxis = axis2.normalize();
    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]);
    frameInW.setOrigin(anchor);
    // now get constraint frame in local coordinate systems
    m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
    m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
    // sei limits
    setLinearLowerLimit(btVector3(0., 0., 0.));
    setLinearUpperLimit(btVector3(0., 0., 0.));
    setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI + UNIV_EPS, -SIMD_PI + UNIV_EPS));
    setAngularUpperLimit(btVector3(0.f,  SIMD_HALF_PI - UNIV_EPS,  SIMD_PI - UNIV_EPS));
}

void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
{
	// the swing axis is computed as the "twist-free" cone rotation,
	// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
	// so, if we're outside the limits, the closest way back inside the cone isn't 
	// along the vector back to the center. better (and more stable) to use the ellipse normal.

	// convert swing axis to direction from center to surface of ellipse
	// (ie. rotate 2D vector by PI/2)
	btScalar y = -vSwingAxis.z();
	btScalar z =  vSwingAxis.y();

	// do the math...
	if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
	{
		// compute gradient/normal of ellipse surface at current "point"
		btScalar grad = y/z;
		grad *= m_swingSpan2 / m_swingSpan1;

		// adjust y/z to represent normal at point (instead of vector to point)
		if (y > 0)
			y =  fabs(grad * z);
		else
			y = -fabs(grad * z);

		// convert ellipse direction back to swing axis
		vSwingAxis.setZ(-y);
		vSwingAxis.setY( z);
		vSwingAxis.normalize();
	}
}

// given a cone rotation in constraint space, (pre: twist must already be removed)
// this method computes its corresponding swing angle and axis.
// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
												 btScalar& swingAngle, // out
												 btVector3& vSwingAxis, // out
												 btScalar& swingLimit) // out
{
	swingAngle = qCone.getAngle();
	if (swingAngle > SIMD_EPSILON)
	{
		vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
		vSwingAxis.normalize();
#if 0
        // non-zero twist?! this should never happen.
       btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
#endif
        
		// Compute limit for given swing. tricky:
		// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
		// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)

		// For starters, compute the direction from center to surface of ellipse.
		// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
		// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
		btScalar xEllipse =  vSwingAxis.y();
		btScalar yEllipse = -vSwingAxis.z();

		// Now, we use the slope of the vector (using x/yEllipse) and find the length
		// of the line that intersects the ellipse:
		//  x^2   y^2
		//  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
		//  a^2   b^2
		// Do the math and it should be clear.

		swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
		if (fabs(xEllipse) > SIMD_EPSILON)
		{
			btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
			btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
			norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
			btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
			swingLimit = sqrt(swingLimit2);
		}

		// test!
		/*swingLimit = m_swingSpan2;
		if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
		{
		btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
		btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
		btScalar phi = asin(sinphi);
		btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
		btScalar alpha = 3.14159f - theta - phi;
		btScalar sinalpha = sin(alpha);
		swingLimit = m_swingSpan1 * sinphi/sinalpha;
		}*/
	}

bool	Raytracer::lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
{

	btScalar closestHitResults = 1.f;

	bool hasHit = false;
	btConvexCast::CastResult rayResult;
	btSphereShape pointShape(0.0f);
	btTransform rayFromTrans;
	btTransform rayToTrans;

	rayFromTrans.setIdentity();
	rayFromTrans.setOrigin(rayFrom);
	rayToTrans.setIdentity();
	rayToTrans.setOrigin(rayTo);

	for (int s=0;s<numObjects;s++)
	{
		
		//do some culling, ray versus aabb
		btVector3 aabbMin,aabbMax;
		shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax);
		btScalar hitLambda = 1.f;
		btVector3 hitNormal;
		btCollisionObject	tmpObj;
		tmpObj.setWorldTransform(transforms[s]);


		if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
		{
			//reset previous result

			//choose the continuous collision detection method
			btSubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
			//btGjkConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
			//btContinuousConvexCollision convexCaster(&pointShape,shapePtr[s],&simplexSolver,0);

			if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult))
			{
				if (rayResult.m_fraction < closestHitResults)
				{
					closestHitResults = rayResult.m_fraction;

					worldNormal = transforms[s].getBasis() *rayResult.m_normal;
					worldNormal.normalize();
					hasHit = true;
				}
			}
		}
	}

	return hasHit;

}

// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2)
: btGeneric6DofSpringConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true),
 m_anchor(anchor),
 m_axis1(axis1),
 m_axis2(axis2)
{
	// build frame basis
	// 6DOF constraint uses Euler angles and to define limits
	// it is assumed that rotational order is :
	// Z - first, allowed limits are (-PI,PI);
	// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
	// used to prevent constraint from instability on poles;
	// new position of X, allowed limits are (-PI,PI);
	// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
	// Build the frame in world coordinate system first
	btVector3 zAxis = axis1.normalize();
	btVector3 xAxis = axis2.normalize();
	btVector3 yAxis = zAxis.cross(xAxis); // 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]);
	frameInW.setOrigin(anchor);
	// now get constraint frame in local coordinate systems
	m_frameInA = rbA.getCenterOfMassTransform().inverse() * frameInW;
	m_frameInB = rbB.getCenterOfMassTransform().inverse() * frameInW;
	// sei limits
	setLinearLowerLimit(btVector3(0.f, 0.f, -1.f));
	setLinearUpperLimit(btVector3(0.f, 0.f,  1.f));
	// like front wheels of a car
	setAngularLowerLimit(btVector3(1.f,  0.f, -SIMD_HALF_PI * 0.5f));
	setAngularUpperLimit(btVector3(-1.f, 0.f,  SIMD_HALF_PI * 0.5f));
	// enable suspension
	enableSpring(2, true);
	setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
	setDamping(2, 0.01f);
	setEquilibriumPoint();
}

bool	Raytracer::singleObjectRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
{

	btScalar closestHitResults = 1.f;

	ClosestRayResultCallback resultCallback(rayFrom,rayTo);

	bool hasHit = false;
	btConvexCast::CastResult rayResult;
	btSphereShape pointShape(0.0f);
	btTransform rayFromTrans;
	btTransform rayToTrans;

	rayFromTrans.setIdentity();
	rayFromTrans.setOrigin(rayFrom);
	rayToTrans.setIdentity();
	rayToTrans.setOrigin(rayTo);

	for (int s=0;s<numObjects;s++)
	{
		//comment-out next line to get all hits, instead of just the closest hit
		//resultCallback.m_closestHitFraction = 1.f;

		//do some culling, ray versus aabb
		btVector3 aabbMin,aabbMax;
		shapePtr[s]->getAabb(transforms[s],aabbMin,aabbMax);
		btScalar hitLambda = 1.f;
		btVector3 hitNormal;
		btCollisionObject	tmpObj;
		tmpObj.setWorldTransform(transforms[s]);


		if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
		{
			//reset previous result

			btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans, &tmpObj, shapePtr[s], transforms[s], resultCallback);
			if (resultCallback.hasHit())
			{
				//float fog = 1.f - 0.1f * rayResult.m_fraction;
				resultCallback.m_hitNormalWorld.normalize();//.m_normal.normalize();
				worldNormal = resultCallback.m_hitNormalWorld;
				//worldNormal = transforms[s].getBasis() *rayResult.m_normal;
				worldNormal.normalize();
				hasHit = true;
			}
		}
	}

	return hasHit;
}

static void MotionCallback(int x, int y)
{
	int dx = mx - x;
	int dy = my - y;
	
	Dir = Dir.normalize();
	N = Dir.cross(btVector3(0,1,0));

	NxQuat qx(NxPiF32 * dx * 20/ 180.0f, btVector3(0,1,0));
	qx.rotate(Dir);
	NxQuat qy(NxPiF32 * dy * 20/ 180.0f, N);
	qy.rotate(Dir);

	mx = x;
	my = y;
}

// dir = 1 for CCW search or clockwise around obstacle
// dir = -1 for CW search or counter clockwise around obstacle
bool cSpace::movePointAroundCSpace(btVector3& pt, btVector3 startVect, float stpMag, int dir)
{
	float angle=0;
	float zHeight = pt.z();
	pt.setZ(0);
	btVector3 nudge = stpMag * startVect.normalize();
	btVector3 npt = pt + nudge;
	
	while(isPointInsideCSpace(npt))							// check if the point is inside of the object
	{
		npt = pt + nudge.rotate(btVector3(0,0,1),angle);	// calculate a new point a radius of vector nudge away at angle
		angle += (dir * 0.1);								// in radians
		
		if(fabs(angle) > TWOPI)								// if the search has gone a full rotation
			return false;									// and there is no path outside a C-Space return false, stuck
	}
	
	pt = npt;
	pt.setZ(zHeight);
	return true;
}

// given a twist rotation in constraint space, (pre: cone must already be removed)
// this method computes its corresponding angle and axis.
void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
												  btScalar& twistAngle, // out
												  btVector3& vTwistAxis) // out
{
	btQuaternion qMinTwist = qTwist;
	twistAngle = qTwist.getAngle();

	if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
	{
		qMinTwist = operator-(qTwist);
		twistAngle = qMinTwist.getAngle();
	}
	if (twistAngle < 0)
	{
		// this should never happen
		int wtf = 0; wtf = wtf;			
	}

	vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
	if (twistAngle > SIMD_EPSILON)
		vTwistAxis.normalize();
}

///combined discrete/continuous sphere-triangle
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
{

    const btVector3* vertices = &m_triangle->getVertexPtr(0);
    const btVector3& c = sphereCenter;
    btScalar r = m_sphere->getRadius();

    btVector3 delta (0,0,0);

    btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
    normal.normalize();
    btVector3 p1ToCentre = c - vertices[0];
    btScalar distanceFromPlane = p1ToCentre.dot(normal);

    if (distanceFromPlane < btScalar(0.))
    {
        //triangle facing the other way

        distanceFromPlane *= btScalar(-1.);
        normal *= btScalar(-1.);
    }

    btScalar contactMargin = contactBreakingThreshold;
    bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
    bool isInsideShellPlane = distanceFromPlane < r;

    btScalar deltaDotNormal = delta.dot(normal);
    if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
        return false;

    // Check for contact / intersection
    bool hasContact = false;
    btVector3 contactPoint;
    if (isInsideContactPlane) {
        if (facecontains(c,vertices,normal)) {
            // Inside the contact wedge - touches a point on the shell plane
            hasContact = true;
            contactPoint = c - normal*distanceFromPlane;
        } else {
            // Could be inside one of the contact capsules
            btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
            btVector3 nearestOnEdge;
            for (int i = 0; i < m_triangle->getNumEdges(); i++) {

                btVector3 pa;
                btVector3 pb;

                m_triangle->getEdge(i,pa,pb);

                btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
                if (distanceSqr < contactCapsuleRadiusSqr) {
                    // Yep, we're inside a capsule
                    hasContact = true;
                    contactPoint = nearestOnEdge;
                }

            }
        }
    }

    if (hasContact) {
        btVector3 contactToCentre = c - contactPoint;
        btScalar distanceSqr = contactToCentre.length2();
        if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
            btScalar distance = btSqrt(distanceSqr);
            resultNormal = contactToCentre;
            resultNormal.normalize();
            point = contactPoint;
            depth = -(r-distance);
            return true;
        }

        if (delta.dot(contactToCentre) >= btScalar(0.0))
            return false;

        // Moving towards the contact point -> collision
        point = contactPoint;
        timeOfImpact = btScalar(0.0);
        return true;
    }

    return false;
}

bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
{

	const btVector3* vertices = &m_triangle->getVertexPtr(0);
	
	btScalar radius = m_sphere->getRadius();
	btScalar radiusWithThreshold = radius + contactBreakingThreshold;

	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
	normal.normalize();
	btVector3 p1ToCentre = sphereCenter - vertices[0];
	btScalar distanceFromPlane = p1ToCentre.dot(normal);

	if (distanceFromPlane < btScalar(0.))
	{
		//triangle facing the other way
		distanceFromPlane *= btScalar(-1.);
		normal *= btScalar(-1.);
	}

	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
	
	// Check for contact / intersection
	bool hasContact = false;
	btVector3 contactPoint;
	if (isInsideContactPlane) {
		if (facecontains(sphereCenter,vertices,normal)) {
			// Inside the contact wedge - touches a point on the shell plane
			hasContact = true;
			contactPoint = sphereCenter - normal*distanceFromPlane;
		} else {
			// Could be inside one of the contact capsules
			btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
			btVector3 nearestOnEdge;
			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
				
				btVector3 pa;
				btVector3 pb;
				
				m_triangle->getEdge(i,pa,pb);

				btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
				if (distanceSqr < contactCapsuleRadiusSqr) {
					// Yep, we're inside a capsule
					hasContact = true;
					contactPoint = nearestOnEdge;
				}
				
			}
		}
	}

	if (hasContact) {
		btVector3 contactToCentre = sphereCenter - contactPoint;
		btScalar distanceSqr = contactToCentre.length2();

		if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
		{
			if (distanceSqr>SIMD_EPSILON)
			{
				btScalar distance = btSqrt(distanceSqr);
				resultNormal = contactToCentre;
				resultNormal.normalize();
				point = contactPoint;
				depth = -(radius-distance);
			} else
			{
				btScalar distance = 0.f;
				resultNormal = normal;
				point = contactPoint;
				depth = -radius;
			}
			return true;
		}
	}
	
	return false;
}