C++ btFabs函数代码示例

btScalar Tire::PacejkaMz(btScalar sigma, btScalar alpha, btScalar Fz, btScalar gamma, btScalar friction_coeff, btScalar & max_Mz) const
{
	const std::vector<btScalar> & c = aligning;

	btScalar C = c[0];

	// peak factor
	btScalar D = (c[1] * Fz + c[2]) * Fz;

	// peak factor 1993
	// D = D * (1 - c[18] * gamma * gamma);

	// slope at origin
	btScalar BCD = (c[3] * Fz + c[4]) * Fz * (1.0 - c[6] * btFabs(gamma)) * exp (-c[5] * Fz);

	// stiffness factor
	btScalar B =  BCD / (C * D);

	// curvature factor
	btScalar E = (c[7] * Fz * Fz + c[8] * Fz + c[9]) * (1.0 - c[10] * btFabs(gamma));

	// curvature factor 1993
	// E = (c[7] * Fz * Fz + c[8] * Fz + c[9]) * (1.0 - (c[19] * gamma + c[20]) * sgn(S)) / (1.0 - c[10] * btFabs(gamma));

	// horizontal shift
	btScalar Sh = c[11] * gamma + c[12] * Fz + c[13];

	// horizontal shift 1993
	// Sh = c[11] * Fz + c[12] + c[13] * gamma;

	// vertical shift
	btScalar Sv = (c[14] * Fz * Fz + c[15] * Fz) * gamma + c[16] * Fz + c[17];

	// vertical shift 1993
	// Sv = c[14] * Fz + c[15] + (c[16] * Fz * Fz + c[17] * Fz) * gamma;

	// composite
	btScalar S = alpha + Sh;

	// self-aligning torque
	btScalar Mz = D * sin(c[0] * atan(B * S - E * (B * S - atan(B * S)))) + Sv;

	// scale by surface friction
	Mz = Mz * friction_coeff;
	max_Mz = (D + Sv) * friction_coeff;

	btAssert(Mz == Mz);
	return Mz;
}

btScalar Tire::getSqueal() const
{
	btScalar squeal = 0.0;
	if (vx * vx > 1E-2 && slide * slide > 1E-6)
	{
		btScalar vx_body = vx / slide;
		btScalar vx_ideal = ideal_slide * vx_body;
		btScalar vy_ideal = btTan(-ideal_slip / 180 * M_PI) * vx_body;
		btScalar vx_squeal = btFabs(vx / vx_ideal);
		btScalar vy_squeal = btFabs(vy / vy_ideal);
		// start squeal at 80% of the ideal slide/slip, max out at 160%
		squeal = 1.25 * btMax(vx_squeal, vy_squeal) - 1.0;
		btClamp(squeal, btScalar(0), btScalar(1));
	}
	return squeal;
}

void btPlaneSpace1 (const float4* n, float4* p, float4* q)
{
    if (btFabs(n->z) > SIMDSQRT12) {
        // choose p in y-z plane
        btScalar a = n->y*n->y + n->z*n->z;
        btScalar k = btRecipSqrt (a);
        p->x = 0;
        p->y = -n->z*k;
        p->z = n->y*k;
        // set q = n x p
        q->x = a*k;
        q->y = -n->x*p->z;
        q->z = n->x*p->y;
    }
    else {
        // choose p in x-y plane
        btScalar a = n->x*n->x + n->y*n->y;
        btScalar k = btRecipSqrt (a);
        p->x = -n->y*k;
        p->y = n->x*k;
        p->z = 0;
        // set q = n x p
        q->x = -n->z*p->y;
        q->y = n->z*p->x;
        q->z = a*k;
    }
}

		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
		{
			(void)triangleIndex;
			(void)partId;
			btMatrix3x3 i;
			btVector3 a = triangle[0] - center;
			btVector3 b = triangle[1] - center;
			btVector3 c = triangle[2] - center;
			btScalar volNeg = -btFabs(a.triple(b, c)) * btScalar(1. / 6);
			for (int j = 0; j < 3; j++)
			{
				for (int k = 0; k <= j; k++)
				{
					i[j][k] = i[k][j] = volNeg * (btScalar(0.1) * (a[j] * a[k] + b[j] * b[k] + c[j] * c[k]) + btScalar(0.05) * (a[j] * b[k] + a[k] * b[j] + a[j] * c[k] + a[k] * c[j] + b[j] * c[k] + b[k] * c[j]));
				}
			}
			btScalar i00 = -i[0][0];
			btScalar i11 = -i[1][1];
			btScalar i22 = -i[2][2];
			i[0][0] = i11 + i22;
			i[1][1] = i22 + i00;
			i[2][2] = i00 + i11;
			sum[0] += i[0];
			sum[1] += i[1];
			sum[2] += i[2];
		}

btScalar Tire::getSqueal() const
{
	btScalar squeal = 0;
	if (vx * vx > btScalar(1E-2) && slip * slip > btScalar(1E-6))
	{
		btScalar vx_body = vx / slip;
		btScalar vx_ideal = ideal_slip * vx_body;
		btScalar vy_ideal = ideal_slip_angle * vx_body; //btTan(ideal_slip_angle) * vx_body;
		btScalar vx_squeal = btFabs(vx / vx_ideal);
		btScalar vy_squeal = btFabs(vy / vy_ideal);
		// start squeal at 80% of the ideal slide/slip, max out at 160%
		squeal = btScalar(1.25) * btMax(vx_squeal, vy_squeal) - 1;
		squeal = Clamp(squeal, btScalar(0), btScalar(1));
	}
	return squeal;
}

void	btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
{
	const int numbrushes = planeEquations.size();
	// brute force:
	for (int i=0;i<numbrushes;i++)
	{
		const btVector3& N1 = planeEquations[i];
		

		for (int j=i+1;j<numbrushes;j++)
		{
			const btVector3& N2 = planeEquations[j];
				
			for (int k=j+1;k<numbrushes;k++)
			{

				const btVector3& N3 = planeEquations[k];

				btVector3 n2n3; n2n3 = N2.cross(N3);
				btVector3 n3n1; n3n1 = N3.cross(N1);
				btVector3 n1n2; n1n2 = N1.cross(N2);
				
				if ( ( n2n3.length2() > btScalar(0.0001) ) &&
					 ( n3n1.length2() > btScalar(0.0001) ) &&
					 ( n1n2.length2() > btScalar(0.0001) ) )
				{
					//point P out of 3 plane equations:

					//	d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
					//P =  -------------------------------------------------------------------------  
					//   N1 . ( N2 * N3 )  


					btScalar quotient = (N1.dot(n2n3));
					if (btFabs(quotient) > btScalar(0.000001))
					{
						quotient = btScalar(-1.) / quotient;
						n2n3 *= N1[3];
						n3n1 *= N2[3];
						n1n2 *= N3[3];
						btVector3 potentialVertex = n2n3;
						potentialVertex += n3n1;
						potentialVertex += n1n2;
						potentialVertex *= quotient;

						//check if inside, and replace supportingVertexOut if needed
						if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
						{
							verticesOut.push_back(potentialVertex);
						}
					}
				}
			}
		}
	}
}

btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
{
	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
	int j;

	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
	{
		for (j=0;j<numPoolConstraints;j++)
		{
			const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
			btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
			btAssert(pt);
			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
		
			pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse;
			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
			{
				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse;
			}
			//do a callback here?
		}
	}
	

	numPoolConstraints = m_multiBodyNonContactConstraints.size();

#if 0
	//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
	for (int i=0;i<numPoolConstraints;i++)
	{
		const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];

		btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
		btJointFeedback* fb = constr->getJointFeedback();
		if (fb)
		{
			fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
			fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
			fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
			fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
			
		}

		constr->internalSetAppliedImpulse(c.m_appliedImpulse);
		if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
		{
			constr->setEnabled(false);
		}

	}
#endif 


	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
}

//bilateral constraint between two dynamic objects
void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
                      btRigidBody& body2, const btVector3& pos2,
                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
{
	(void)timeStep;
	(void)distance;


	btScalar normalLenSqr = normal.length2();
	btAssert(btFabs(normalLenSqr) < btScalar(1.1));
	if (normalLenSqr > btScalar(1.1))
	{
		impulse = btScalar(0.);
		return;
	}
	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
	//this jacobian entry could be re-used for all iterations
	
	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
	btVector3 vel = vel1 - vel2;
	

	   btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
		body2.getCenterOfMassTransform().getBasis().transpose(),
		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
		body2.getInvInertiaDiagLocal(),body2.getInvMass());

	btScalar jacDiagAB = jac.getDiagonal();
	btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
	
	  btScalar rel_vel = jac.getRelativeVelocity(
		body1.getLinearVelocity(),
		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
		body2.getLinearVelocity(),
		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
	btScalar a;
	a=jacDiagABInv;


	rel_vel = normal.dot(vel);
	
	//todo: move this into proper structure
	btScalar contactDamping = btScalar(0.2);

#ifdef ONLY_USE_LINEAR_MASS
	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
	impulse = - contactDamping * rel_vel * massTerm;
#else	
	btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
	impulse = velocityImpulse;
#endif
}

static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
{
	btScalar tol(0.3);
	btScalar result = btShortestAngularDistance(accAngle, curAngle);

	  if (btFabs(result) > tol)
		return curAngle;
	  else
		return accAngle + result;

	return curAngle;
}

 void CubeGeometry::setColorBase(Node *node)
 {
     unsigned long index = getGeometryIndex(node);
     
     if(m_VertexData)
     {
         
         btVector4 c(btFabs(node->getColorBase().x()),
                     btFabs(node->getColorBase().y()),
                     btFabs(node->getColorBase().z()),
                     btFabs(node->getColorBase().w()));
         
         m_VertexData[index].blf.color = c;
         m_VertexData[index].brf.color = c;
         m_VertexData[index].tlf.color = c;
         m_VertexData[index].trf.color = c;
         m_VertexData[index].blb.color = c;
         m_VertexData[index].brb.color = c;
         m_VertexData[index].tlb.color = c;
         m_VertexData[index].trb.color = c;
     }
 }

void Tire::findSigmaHatAlphaHat(
	btScalar load,
	btScalar & output_sigmahat,
	btScalar & output_alphahat,
	int iterations)
{
	btScalar Fz = load;
	btScalar Fz0 = nominal_load;
	btScalar dFz = (Fz - Fz0) / Fz0;
	btScalar camber = 0.0;
	btScalar mu = 1.0;
	btScalar Dy, BCy, Shf; // unused

	btScalar Fxmax = 0.0;
	btScalar smax = 2.0;
	for (btScalar s = -smax; s < smax; s += 2 * smax / iterations)
	{
		btScalar Fx = PacejkaFx(s, Fz, dFz, mu);
		if (Fx > Fxmax)
		{
			output_sigmahat = btFabs(s);
			Fxmax = Fx;
		}
	}

	btScalar Fymax = 0.0;
	btScalar amax = 30.0 * (M_PI / 180.0);
	for (btScalar a = -amax; a < amax; a += 2 * amax / iterations)
	{
		btScalar Fy = PacejkaFy(a, camber, Fz, dFz, mu, Dy, BCy, Shf);
		if (Fy > Fymax)
		{
			output_alphahat = btFabs(a);
			Fymax = Fy;
		}
	}
}

btScalar Tire::PacejkaFy(btScalar alpha, btScalar Fz, btScalar gamma, btScalar friction_coeff, btScalar & max_Fy) const
{
	const std::vector<btScalar> & a = lateral;

	// shape factor
	btScalar C = a[0];

	// peak factor
	btScalar D = (a[1] * Fz + a[2]) * Fz;

	// peak factor 1993
	// D = D * (1 - a[15] * gamma * gamma);

	// slope at origin
	btScalar BCD = a[3] * sin(2.0 * atan(Fz / a[4])) * (1.0 - a[5] * btFabs(gamma));

	// stiffness factor
	btScalar B = BCD / (C * D);

	// curvature factor
	btScalar E = a[6] * Fz + a[7];

	// curvature factor 1993
	// E = E * (1 - (a[16] * gamma + a[17]) * sgn(alpha + Sh));

	// horizontal shift
	btScalar Sh = a[8] * gamma + a[9] * Fz + a[10];

	// horizontal shift 1993
	// Sh = a[8] * Fz + a[9] + a[10] * gamma;

	// vertical shift
	btScalar Sv = ((a[11] * Fz + a[12]) * gamma + a[13]) * Fz + a[14];

	// vertical shift 1993
	// Sv = a[11] * Fz + a[12] + (a[13] * Fz * Fz + a[14] * Fz) * gamma;

	// composite
	btScalar S = alpha + Sh;

	// lateral force
	btScalar Fy = D * sin(C * atan(B * S - E * (B * S - atan(B * S)))) + Sv;

	// scale by surface friction
	Fy = Fy * friction_coeff;
	max_Fy = (D + Sv) * friction_coeff;

	return Fy;
}

void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
{
	// it is assumed that calculateTransforms() have been called before this call
	int i;
	btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
	for(i = 0; i < 3; i++)
	{
		if(m_springEnabled[i])
		{
			// get current position of constraint
			btScalar currPos = m_calculatedLinearDiff[i];
			// calculate difference
			btScalar delta = currPos - m_equilibriumPoint[i];
			// spring force is (delta * m_stiffness) according to Hooke's Law
			btScalar force = delta * m_springStiffness[i];
			btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
			m_linearLimits.m_targetVelocity[i] =  velFactor * force;
			m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
		}
	}
	for(i = 0; i < 3; i++)
	{
		if(m_springEnabled[i + 3])
		{
			// get current position of constraint
			btScalar currPos = m_calculatedAxisAngleDiff[i];
			// calculate difference
			btScalar delta = currPos - m_equilibriumPoint[i+3];
			// spring force is (-delta * m_stiffness) according to Hooke's Law
			btScalar force = -delta * m_springStiffness[i+3];
			btScalar velFactor = info->fps * m_springDamping[i+3] / btScalar(info->m_numIterations);
			m_angularLimits[i].m_targetVelocity = velFactor * force;
			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
		}
	}
}

void Voronoi::getVerticesInsidePlanes(const vector<btVector3>& planes, vector<btVector3>& verticesOut, std::set<int>& planeIndicesOut)
{
    // Based on btGeometryUtil.cpp (Gino van den Bergen / Erwin Coumans)
    verticesOut.resize(0);
    planeIndicesOut.clear();
    const int numPlanes = planes.size();
    int i, j, k, l;
    for (i=0;i<numPlanes;i++)
    {
        const btVector3& N1 = planes[i];
        for (j=i+1;j<numPlanes;j++)
        {
            const btVector3& N2 = planes[j];
            btVector3 n1n2 = N1.cross(N2);
            if (n1n2.length2() > btScalar(0.0001))
            {
                for (k=j+1;k<numPlanes;k++)
                {
                    const btVector3& N3 = planes[k];
                    btVector3 n2n3 = N2.cross(N3);
                    btVector3 n3n1 = N3.cross(N1);
                    if ((n2n3.length2() > btScalar(0.0001)) && (n3n1.length2() > btScalar(0.0001) ))
                    {
                        btScalar quotient = (N1.dot(n2n3));
                        if (btFabs(quotient) > btScalar(0.0001))
                        {
                            btVector3 potentialVertex = (n2n3 * N1[3] + n3n1 * N2[3] + n1n2 * N3[3]) * (btScalar(-1.) / quotient);
                            for (l=0; l<numPlanes; l++)
                            {
                                const btVector3& NP = planes[l];
                                if (btScalar(NP.dot(potentialVertex))+btScalar(NP[3]) > btScalar(0.000001))
                                    break;
                            }
                            if (l == numPlanes)
                            {
                                // vertex (three plane intersection) inside all planes
                                verticesOut.push_back(potentialVertex);
                                planeIndicesOut.insert(i);
                                planeIndicesOut.insert(j);
                                planeIndicesOut.insert(k);
                            }
                        }
                    }
                }
            }
        }
    }
}

//bilateral constraint between two dynamic objects
void RaycastCar::resolveSingleBilateral(btRigidBody     & body1,
                                        const btVector3 & pos1,
                                        btRigidBody     & body2,
                                        const btVector3 & pos2,
                                        const btVector3 & normal,
                                        btScalar        & impulse)
{
    btScalar normalLenSqr = normal.length2();
    btAssert(btFabs(normalLenSqr) < btScalar(1.1f));
    if (normalLenSqr > btScalar(1.1f))
    {
        impulse = btScalar(0.0f);
        return;
    }
    btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition();
    btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
    btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
                        body2.getCenterOfMassTransform().getBasis().transpose(),
                        rel_pos1,
                        rel_pos2,
                        normal,
                        body1.getInvInertiaDiagLocal(),
                        body1.getInvMass(),
                        body2.getInvInertiaDiagLocal(),
                        body2.getInvMass());

    btScalar jacDiagAB = jac.getDiagonal();
    btScalar jacDiagABInv = btScalar(1.0f) / jacDiagAB;

    btScalar rel_vel = jac.getRelativeVelocity
                       (body1.getLinearVelocity(),
                        body1.getCenterOfMassTransform().getBasis().transpose()*body1.getAngularVelocity(),
                        body2.getLinearVelocity(),
                        body2.getCenterOfMassTransform().getBasis().transpose()*body2.getAngularVelocity());

    btScalar velocityImpulse = -1.0f * rel_vel * jacDiagABInv;
    impulse = velocityImpulse;
}