C++ btSqrt函数代码示例

Vehicle::Vehicle():
	m_carChassis(0)
	{
		rightIndex = 0;
		upIndex = 1;
		forwardIndex = 2;

		gEngineForce = 0.f;
		gBreakingForce = 0.f;
		maxEngineForce = 100000.f; //1500.f; //this should be engine/velocity dependent
		maxBreakingForce = 8000.f;  // 100.f; 
		gVehicleSteering = 0.f;
		steeringIncrement =  1.0f; //0.04f;
		steeringClamp = 0.3f;
		wheelRadius =  0.35f * PHYSCAR_SCALE;	//** 0.5f;
		wheelWidth = 0.2f * PHYSCAR_SCALE;		//* 0.4f;
		wheelFriction = 10;						// 1000;//BT_LARGE_FLOAT;
		suspensionStiffness = 50.f;				// 20.f;
		suspensionDamping =	 0.3f * 2.0f * btSqrt(suspensionStiffness);	// 2.3f;
		suspensionCompression = 0.2f * 2.0f * btSqrt(suspensionStiffness);	// 4.4f;
		rollInfluence = 0.5f;
		wheelDirectionCS0 = btVector3(0,-1,0);
		wheelAxleCS = btVector3(-1,0,0);
		suspensionRestLength = 0.15f * PHYSCAR_SCALE;  //** (0.6);

		m_vehicle = 0;
		m_wheelShape = 0;
		m_vehicleRayCaster = NULL;
		m_vehicle = NULL;
		m_compound = NULL;
}

static void M3x3getRot_ref(const btMatrix3x3 &m, btQuaternion &q)
{
	btVector3 m_el[3] = {m[0], m[1], m[2]};

	btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();

	btScalar temp[4];

	if (trace > btScalar(0.0))
	{
		btScalar s = btSqrt(trace + btScalar(1.0));
		temp[3] = (s * btScalar(0.5));
		s = btScalar(0.5) / s;

		temp[0] = ((m_el[2].y() - m_el[1].z()) * s);
		temp[1] = ((m_el[0].z() - m_el[2].x()) * s);
		temp[2] = ((m_el[1].x() - m_el[0].y()) * s);
	}
	else
	{
		int i = m_el[0].x() < m_el[1].y() ? (m_el[1].y() < m_el[2].z() ? 2 : 1) : (m_el[0].x() < m_el[2].z() ? 2 : 0);
		int j = (i + 1) % 3;
		int k = (i + 2) % 3;

		btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
		temp[i] = s * btScalar(0.5);
		s = btScalar(0.5) / s;

		temp[3] = (m_el[k][j] - m_el[j][k]) * s;
		temp[j] = (m_el[j][i] + m_el[i][j]) * s;
		temp[k] = (m_el[k][i] + m_el[i][k]) * s;
	}
	q.setValue(temp[0], temp[1], temp[2], temp[3]);
}

btVector3	btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
	int i;
	btVector3 supVec(0,0,0);

	btScalar maxDot(btScalar(-1e30));

	btVector3 vec = vec0;
	btScalar lenSqr = vec.length2();
	if (lenSqr < btScalar(0.0001))
	{
		vec.setValue(1,0,0);
	} else
	{
		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
		vec *= rlen;
	}

	btVector3 vtx;
	btScalar newDot;

	for (i=0;i<getNumVertices();i++)
	{
		getVertex(i,vtx);
		newDot = vec.dot(vtx);
		if (newDot > maxDot)
		{
			maxDot = newDot;
			supVec = vtx;
		}
	}

	return supVec;

}

btVector3	btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
	btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);

	btVector3 vec = vec0;
	btScalar lenSqr = vec.length2();
	if (lenSqr < btScalar(0.0001))
	{
		vec.setValue(1,0,0);
	} else
	{
		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
		vec *= rlen;
	}


	for (int i=0;i<m_unscaledPoints.size();i++)
	{
		btVector3 vtx = m_unscaledPoints[i] * m_localScaling;

		newDot = vec.dot(vtx);
		if (newDot > maxDot)
		{
			maxDot = newDot;
			supVec = vtx;
		}
	}
	return supVec;
}

inline  btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v) 
{

const int cylinderUpAxis = 1;
const int XX = 0;
const int YY = 1;
const int ZZ = 2;


	btScalar radius = halfExtents[XX];
	btScalar halfHeight = halfExtents[cylinderUpAxis];


    btVector3 tmp;
	btScalar d ;

    btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
    if (s != btScalar(0.0))
	{
        d = radius / s;  
		tmp[XX] = v[XX] * d;
		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
		tmp[ZZ] = v[ZZ] * d;
		return tmp;
	}
    else
	{
	    tmp[XX] = radius;
		tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
		tmp[ZZ] = btScalar(0.0);
		return tmp;
    }

}

    void EXPORT_API ProjectSphereCollisionConstraints(btVector3* predPosA, bool* posLocksA, int pointsCountA, float radiusA, float invMassA,
            btVector3* predPosB, bool* posLocksB, int pointsCountB, float radiusB, float invMassB, float Ks_prime)
    {
        //  float Ks_prime  = 1.0f - Mathf.Pow((1.0f - Ks),  1.0f / solverIterations);

        float radiusSum = radiusA + radiusB;
        float radiusSumSq = radiusSum * radiusSum;

        //#pragma omp parallel for
        for (int idA = 0; idA < pointsCountA; idA++)
        {
            for (int idB = 0; idB < pointsCountB; idB++)
            {
                btVector3 dir = predPosA[idA] - predPosB[idB];
                float distanceSq = dir.length2();

                if (distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
                    continue;

                float distance = btSqrt(distanceSq);

                btScalar w1 = posLocksA[idA] ? 0.0f : invMassA;
                btScalar w2 = posLocksB[idB] ? 0.0f : invMassB;

                float invMass = w1 + w2;

                btVector3 dP = (1.0f / invMass) * (distance - radiusSum) * (dir / distance) * Ks_prime;

                predPosA[idA] -= dP * w1;
                predPosB[idB] += dP * w2;
            }

        }

    }

btVector3	btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
	btScalar maxDot = btScalar(-BT_LARGE_FLOAT);

	btVector3 vec = vec0;
	btScalar lenSqr = vec.length2();
	if (lenSqr < btScalar(0.0001))
	{
		vec.setValue(1,0,0);
	} else
	{
		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
		vec *= rlen;
	}
    
    if( m_numPoints > 0 )
    {
        // Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically.
    //    btVector3 scaled = vec * m_localScaling;
        int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot);   //FIXME: may violate encapsulation of m_unscaledPoints
        return getScaledPoint(index);
    }

	return supVec;
}

		void getInfo2 (btConstraintInfo2* info) {
			btVector3 relA = m_rbA.getCenterOfMassTransform().getBasis() * getPivotInA();
			btVector3 relB = m_rbB.getCenterOfMassTransform().getBasis() * getPivotInB();
			btVector3 posA = m_rbA.getCenterOfMassTransform().getOrigin() + relA;
			btVector3 posB = m_rbB.getCenterOfMassTransform().getOrigin() + relB;
			btVector3 del = posB - posA;
			btScalar currDist = btSqrt(del.dot(del));
			btVector3 ortho = del / currDist;
			info->m_J1linearAxis[0] = ortho[0];
			info->m_J1linearAxis[1] = ortho[1];
			info->m_J1linearAxis[2] = ortho[2];
			btVector3 p, q;
			p = relA.cross(ortho);
			q = relB.cross(ortho);
			info->m_J1angularAxis[0] = p[0];
			info->m_J1angularAxis[1] = p[1];
			info->m_J1angularAxis[2] = p[2];
			info->m_J2angularAxis[0] = -q[0];
			info->m_J2angularAxis[1] = -q[1];
			info->m_J2angularAxis[2] = -q[2];
			btScalar rhs = (currDist - m_dist) * info->fps * info->erp;
			info->m_constraintError[0] = rhs;
			info->cfm[0] = btScalar(0.f);
			info->m_lowerLimit[0] = -SIMD_INFINITY;
			info->m_upperLimit[0] = SIMD_INFINITY;
		}

btVector3	btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
	btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
	btScalar newDot,maxDot = btScalar(-1e30);

	btVector3 vec = vec0;
	btScalar lenSqr = vec.length2();
	if (lenSqr < btScalar(0.0001))
	{
		vec.setValue(1,0,0);
	} else
	{
		btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
		vec *= rlen;
	}


	for (int i=0;i<m_numPoints;i++)
	{
		btVector3 vtx = getScaledPoint(i);

		newDot = vec.dot(vtx);
		if (newDot > maxDot)
		{
			maxDot = newDot;
			supVec = vtx;
		}
	}
	return supVec;
}

btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
{
	
	btScalar halfHeight = m_height * btScalar(0.5);

 if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
 {
	btVector3 tmp;

	tmp[m_coneIndices[0]] = btScalar(0.);
	tmp[m_coneIndices[1]] = halfHeight;
	tmp[m_coneIndices[2]] = btScalar(0.);
	return tmp;
 }
  else {
    btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
    if (s > SIMD_EPSILON) {
      btScalar d = m_radius / s;
	  btVector3 tmp;
	  tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
	  tmp[m_coneIndices[1]] = -halfHeight;
	  tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
	  return tmp;
    }
    else  {
		btVector3 tmp;
		tmp[m_coneIndices[0]] = btScalar(0.);
		tmp[m_coneIndices[1]] = -halfHeight;
		tmp[m_coneIndices[2]] = btScalar(0.);
		return tmp;
	}
  }

}

	void EXPORT_API  ComputeSphereCollisionsDEM(btVector3* positions, btVector3* velocities, btVector3* forces, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float radius, float spring, float damp, float shear, float attraction)
	{

		float radiusSum = radius + radius;
		float radiusSumSq = radiusSum * radiusSum;

		#pragma omp parallel
		{

			#pragma omp for
			for (int idA = 0; idA < pointsCount; idA++)
			{

				btVector3 force(0, 0, 0);

				for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
				{
					int idB = neighbours[idA * maxNeighboursPerPoint + nId];

					// calculate relative position
					btVector3 relPos = positions[idB] - positions[idA];

					float distanceSq = relPos.length2();

					if (idA == idB || distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
						continue;

					float dist = btSqrt(distanceSq);

					//btScalar w1 = posLocks[idA] ? 0.0f : massInv;
					//btScalar w2 = posLocks[idB] ? 0.0f : massInv;

					float collideDist = radiusSum;

					if (dist < collideDist)
					{
						btVector3 norm = relPos / dist;

						// relative velocity
						btVector3 relVel = velocities[idB] - velocities[idA];

						// relative tangential velocity
						btVector3 tanVel = relVel - (btDot(relVel, norm) * norm);

						force += -spring*(collideDist - dist) * norm;
						force += damp*relVel;
						force += shear*tanVel;
						force += attraction*relPos;
					}

				}

				forces[idA] += force;

			}
		}


	}

void CarHandlingDemo::addWheels(
	btVector3* halfExtents,
	btRaycastVehicle* vehicle,
	btRaycastVehicle::btVehicleTuning tuning)
{
	//The direction of the raycast, the btRaycastVehicle uses raycasts instead of simiulating the wheels with rigid bodies
	btVector3 wheelDirectionCS0(0, -1, 0);

	//The axis which the wheel rotates arround
	btVector3 wheelAxleCS(-1, 0, 0);

	btScalar suspensionRestLength(0.7);

	btScalar wheelWidth(0.4);

	btScalar wheelRadius(0.5);

	//The height where the wheels are connected to the chassis
	btScalar connectionHeight(1.2);

	//All the wheel configuration assumes the vehicle is centered at the origin and a right handed coordinate system is used
	btVector3 wheelConnectionPoint(halfExtents->x() - wheelRadius, connectionHeight, halfExtents->z() - wheelWidth);

	//Adds the front wheels
	vehicle->addWheel(wheelConnectionPoint, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, true);

	vehicle->addWheel(wheelConnectionPoint * btVector3(-1, 1, 1), wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, true);

	//Adds the rear wheels
	vehicle->addWheel(wheelConnectionPoint* btVector3(1, 1, -1), wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, false);

	vehicle->addWheel(wheelConnectionPoint * btVector3(-1, 1, -1), wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, false);

	//Configures each wheel of our vehicle, setting its friction, damping compression, etc.
	//For more details on what each parameter does, refer to the docs
	for (int i = 0; i < vehicle->getNumWheels(); i++)
	{
		btWheelInfo& wheel = vehicle->getWheelInfo(i);
		wheel.m_suspensionStiffness = 50;
		wheel.m_wheelsDampingCompression = btScalar(0.3) * 2 * btSqrt(wheel.m_suspensionStiffness);//btScalar(0.8);
		wheel.m_wheelsDampingRelaxation = btScalar(0.5) * 2 * btSqrt(wheel.m_suspensionStiffness);//1;
		//Larger friction slips will result in better handling
		wheel.m_frictionSlip = btScalar(1.2);
		wheel.m_rollInfluence = 1;
	}
}

	void EXPORT_API ProjectInternalCollisionConstraintsMT(btVector3* predictedPositions, bool* posLocks, btVector3* temp, int pointsCount, int* neighbours, int* pointNeighboursCount, int maxNeighboursPerPoint, float Ks_prime, float radius, float mass)
	{
		btScalar massInv = 1.0f / mass;
		//  float Ks_prime  = 1.0f - Mathf.Pow((1.0f - Ks),  1.0f / solverIterations);

		float radiusSum = radius + radius;
		float radiusSumSq = radiusSum * radiusSum;

		//#pragma omp parallel num_threads(3)
		#pragma omp parallel
		{

			#pragma omp for
			for (int idA = 0; idA < pointsCount; idA++)
			{
				btVector3 deltaP(0, 0, 0);
				int collisionsCount = 0;
				for (int nId = 0; nId < pointNeighboursCount[idA]; nId++)
				{
					int idB = neighbours[idA * maxNeighboursPerPoint + nId];

					btVector3 dir = predictedPositions[idA] - predictedPositions[idB];
					float distanceSq = dir.length2();

					if (idA == idB || distanceSq > radiusSumSq || distanceSq <= FLT_EPSILON)
						continue;

					float distance = btSqrt(distanceSq);

					btScalar w1 = posLocks[idA] ? 0.0f : massInv;
					btScalar w2 = posLocks[idB] ? 0.0f : massInv;

					float invMass = w1 + w2;

					btVector3 dP = (1.0f / invMass) * (distance - radiusSum) * (dir / distance) * Ks_prime;
					deltaP -= dP * w1 / (btScalar)pointNeighboursCount[idA];
					collisionsCount++;

					//	predictedPositions[idA] -= dP * w1;
					//	predictedPositions[idB] += dP * w2;
				}

				temp[idA] = deltaP;

				//if (collisionsCount > 0)
				//	temp[idA] = deltaP / (btScalar)collisionsCount;
				//else
				//	temp[idA] = btVector3(0, 0, 0);
			}

			#pragma omp for
			for (int i = 0; i < pointsCount; i++)
			{
				predictedPositions[i] += temp[i];
			}

		}
	}

btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (),
m_radius (radius),
m_height(height)
{
	m_shapeType = CONE_SHAPE_PROXYTYPE;
	setConeUpIndex(1);
	btVector3 halfExtents;
	m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
}

void	btConeShape::setLocalScaling(const btVector3& scaling)
{
	int axis = m_coneIndices[1];
	int r1 = m_coneIndices[0];
	int r2 = m_coneIndices[2];
	m_height *= scaling[axis] / m_localScaling[axis];
	m_radius *= (scaling[r1] / m_localScaling[r1] + scaling[r2] / m_localScaling[r2]) / 2;
	m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
	btConvexInternalShape::setLocalScaling(scaling);
}