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

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;
	}
  }

}

float du_get_body_speed(du_body_id body)
{
    btRigidBody *bt_body = reinterpret_cast <btRigidBody*>(body);
    const btVector3 velocity = bt_body->getLinearVelocity();
    float speed_ms = velocity.length();
    float speed = speed_ms *= 3.6;
    return speed;
}

bool btSphereBoxCollisionAlgorithm::getSphereDistance(const btCollisionObjectWrapper* boxObjWrap, btVector3& pointOnBox, btVector3& normal, btScalar& penetrationDepth, const btVector3& sphereCenter, btScalar fRadius, btScalar maxContactDistance ) 
{
	const btBoxShape* boxShape= (const btBoxShape*)boxObjWrap->getCollisionShape();
	btVector3 const &boxHalfExtent = boxShape->getHalfExtentsWithoutMargin();
	btScalar boxMargin = boxShape->getMargin();
	penetrationDepth = 1.0f;

	// convert the sphere position to the box's local space
	btTransform const &m44T = boxObjWrap->getWorldTransform();
	btVector3 sphereRelPos = m44T.invXform(sphereCenter);

	// Determine the closest point to the sphere center in the box
	btVector3 closestPoint = sphereRelPos;
	closestPoint.setX( btMin(boxHalfExtent.getX(), closestPoint.getX()) );
	closestPoint.setX( btMax(-boxHalfExtent.getX(), closestPoint.getX()) );
	closestPoint.setY( btMin(boxHalfExtent.getY(), closestPoint.getY()) );
	closestPoint.setY( btMax(-boxHalfExtent.getY(), closestPoint.getY()) );
	closestPoint.setZ( btMin(boxHalfExtent.getZ(), closestPoint.getZ()) );
	closestPoint.setZ( btMax(-boxHalfExtent.getZ(), closestPoint.getZ()) );
	
	btScalar intersectionDist = fRadius + boxMargin;
	btScalar contactDist = intersectionDist + maxContactDistance;
	normal = sphereRelPos - closestPoint;

	//if there is no penetration, we are done
	btScalar dist2 = normal.length2();
	if (dist2 > contactDist * contactDist)
	{
		return false;
	}

	btScalar distance;

	//special case if the sphere center is inside the box
	if (dist2 == 0.0f)
	{
		distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, closestPoint, normal);
	}
	else //compute the penetration details
	{
		distance = normal.length();
		normal /= distance;
	}

	pointOnBox = closestPoint + normal * boxMargin;
//	v3PointOnSphere = sphereRelPos - (normal * fRadius);	
	penetrationDepth = distance - intersectionDist;

	// transform back in world space
	btVector3 tmp = m44T(pointOnBox);
	pointOnBox = tmp;
//	tmp = m44T(v3PointOnSphere);
//	v3PointOnSphere = tmp;
	tmp = m44T.getBasis() * normal;
	normal = tmp;

	return true;
}

/** Kernel for pressure */
inline float VRFluids::kernel_spiky(btVector3 v, float h) {
    float r = v.length();
    float diff = h - r;
    if (diff > 0) {
        return (15.0 / (Pi * pow(h,6))) * diff*diff*diff;
    } else {
        return 0.0;
    }
}

/**
 *  Kernel_visc laplacian function for viscosity
 *  Source: mueller
 */
inline float VRFluids::kernel_visc_laplacian(btVector3 v, float h) {
    float r = v.length();
    if (r <= h && r >= 0) {
        float diff = h - r;
        return (45.0 / (Pi * pow(h,6))) * diff;
    } else {
        return 0.0;
    }
}

/** Kernel for pressure */
inline btVector3 VRFluids::kernel_spiky_gradient(btVector3 v, float h) {
    float r = v.length();
    float diff = h - r;
    if (diff > 0 && r > 0) { // NOTE > should be >= but things won't work then for some reason
        return (-45.0 / (Pi * pow(h,6))) * (v/r) * diff*diff;
    } else {
        return btVector3(0,0,0);
    }
}

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

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

btVector3 EntityPhysic::colVelocity(btVector3 velocity_)
{

	int MAX=120;
	int MIN=50;
	if(velocity_.length()>MAX)
	{
		float radio=velocity_.length()/MAX;
		velocity_=btVector3(velocity_.getX()/radio,
				velocity_.getY()/radio,
				velocity_.getZ()/radio);
	}
	if(velocity_.length()<MIN)
	{
		float radio=velocity_.length()/MIN;
		velocity_=btVector3(velocity_.getX()/radio,
				velocity_.getY()/radio,
				velocity_.getZ()/radio);
	}
	return velocity_;
}

/** Kernel for viscosity */
inline float VRFluids::kernel_visc(btVector3 v, float h) {
    float r = v.length();
    if (r <= h) {
        float h2 = h*h;
        float h3 = h2*h;
        float r2 = r*r;
        float r3 = r2*r;
        float a = -1*r3 / (h3+h3);
        float b = r2 / h2;
        float c = h / (r+r);
        return (15.0 / (2*Pi*h3)) * (a + b + c - 1);
    } else {
        return 0.0;
    }
}

/**
 * Returns the unit vector in the direction of this spring.
 */
const btVector3 tgBulletCompressionSpring::getAnchorDirectionUnitVector() const
{
    // Get the unit vector for the direction of the force, this is needed for
    // applying the force to the rigid bodies.
    const btVector3 dist =
	  anchor2->getWorldPosition() - anchor1->getWorldPosition();
    // The unit vector of the direction of the force will be needed later
    // In order to have a positive force move the two rigid bodies away
    // from each other, this unit vector must be in the opposite direction
    // of this calculation. Otherwise, a positive force brings them closer
    // together. Needs a minus.
    // note that dist.length is a scalar double.
    const btVector3 unitVector = - dist / dist.length();
    return unitVector;
}

btMatrix3x3 GetOrientedBasis(btVector3 const &z)
{
	btAssert(fabsf(z.length()-1) < 0.01f);
	btVector3 t(0,0,0);
	if(fabsf(z.z() < 0.999f))
	{
		t.setZ(1);
	}
	else
	{
		t.setX(1);
	}
	btVector3 x = t.cross(z).normalize();
	btVector3 y = z.cross(x).normalize();
	return btMatrix3x3(	x.x(), y.x(), z.x(),
						x.y(), y.y(), z.y(),
						x.z(), y.z(), z.z());
}

void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const
{
	//start with static aabb
	getAabb(curTrans,temporalAabbMin,temporalAabbMax);

	btScalar temporalAabbMaxx = temporalAabbMax.getX();
	btScalar temporalAabbMaxy = temporalAabbMax.getY();
	btScalar temporalAabbMaxz = temporalAabbMax.getZ();
	btScalar temporalAabbMinx = temporalAabbMin.getX();
	btScalar temporalAabbMiny = temporalAabbMin.getY();
	btScalar temporalAabbMinz = temporalAabbMin.getZ();

	// add linear motion
	btVector3 linMotion = linvel*timeStep;
	///@todo: simd would have a vector max/min operation, instead of per-element access
	if (linMotion.x() > btScalar(0.))
		temporalAabbMaxx += linMotion.x(); 
	else
		temporalAabbMinx += linMotion.x();
	if (linMotion.y() > btScalar(0.))
		temporalAabbMaxy += linMotion.y(); 
	else
		temporalAabbMiny += linMotion.y();
	if (linMotion.z() > btScalar(0.))
		temporalAabbMaxz += linMotion.z(); 
	else
		temporalAabbMinz += linMotion.z();

	//add conservative angular motion
	btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
	btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
	temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
	temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);

	temporalAabbMin -= angularMotion3d;
	temporalAabbMax += angularMotion3d;
}

void AIInput::step(JeepActor* jeep, btVector3 const & pathDir1, btVector3 const & pathDir2, btVector3 const & trackVector, btVector3 const & segmentVec1, btVector3 const & segmentVec2) {
	btVector3 trackDirection = pathDir1;
	btVector3 actorHeading = quatRotate(jeep->orientation, btVector3(1,0,0)).normalize();

	AcceleratePressed = true;

	float angleToTrack = trackDirection.dot(actorHeading);
	float turnDir = trackDirection.cross(actorHeading).getY();
	if (turnDir > 0) turnDir = 1;
	else if (turnDir < 0) turnDir = -1;
	else turnDir = 0;

	int parallelize = 0;
	float parallelizationThreshold = LoadFloat("config/ai.xml","parallelization_threshold");
	float hardParallelizationThreshold = LoadFloat("config/ai.xml","hard_parallelization_threshold");
	if ((1.0 - angleToTrack) * turnDir > hardParallelizationThreshold) parallelize = 2;
	if ((1.0 - angleToTrack) * turnDir > parallelizationThreshold) parallelize = 1;
	else if ((1.0 - angleToTrack) * turnDir < -hardParallelizationThreshold) parallelize = -2;
	else if ((1.0 - angleToTrack) * turnDir < -parallelizationThreshold) parallelize = -1;

	int onTrack = 0;
	btVector3 pathVec = trackVector;
	float distFromTrack = pathVec.length();

	turnDir = trackDirection.cross(pathVec).getY();
	if (turnDir > 0) turnDir = 1;
	else if (turnDir < 0) turnDir = -1;
	else turnDir = 0;

	float distanceThreshold = LoadFloat("config/ai.xml","distance_threshold");
	float farDistanceThreshold = LoadFloat("config/ai.xml","far_distance_threshold");
	if (distFromTrack > farDistanceThreshold && turnDir > 0)  onTrack = 2;
	if (distFromTrack > distanceThreshold && turnDir > 0) onTrack = 1;
	else if (distFromTrack > farDistanceThreshold && turnDir < 0)  onTrack = -2;
	else if (distFromTrack > distanceThreshold && turnDir < 0) onTrack = -1;
	//std::cout << distFromTrack << std::endl;

	float distToNextSeg = segmentVec1.length();
	float distToNextSeg2 = segmentVec2.length();
	int turnAnticipation1 = 0;
	int turnAnticipation2 = 0;
	//std::cout << distToNextSeg << "\t" << distToNextSeg2 << std::endl;

	float turnAnticipationThreshold = LoadFloat("config/ai.xml","turn_anticipation_threshold");
	float farTurnAnticipationThreshold = LoadFloat("config/ai.xml","far_turn_anticipation_threshold");
	if (distToNextSeg < turnAnticipationThreshold) {
		trackDirection = pathDir1;
		angleToTrack = trackDirection.dot(actorHeading);
		turnDir = trackDirection.cross(actorHeading).getY();
		if (turnDir > 0) turnDir = 1;
		else if (turnDir < 0) turnDir = -1;
		else turnDir = 0;
		
		if ((1.0 - angleToTrack) * turnDir > 0.2) turnAnticipation1 = 2;
		else if ((1.0 - angleToTrack) * turnDir < -0.2) turnAnticipation1 = -2;
	}
	if (distToNextSeg2 < farTurnAnticipationThreshold) {
		trackDirection = pathDir2;
		angleToTrack = trackDirection.dot(actorHeading);
		turnDir = trackDirection.cross(actorHeading).getY();
		if (turnDir > 0) turnDir = 1;
		else if (turnDir < 0) turnDir = -1;
		else turnDir = 0;
		
		if ((1.0 - angleToTrack) * turnDir > 0.2) turnAnticipation2 = 1;
		else if ((1.0 - angleToTrack) * turnDir < -0.2) turnAnticipation2 = -1;
	}
	// std::cout << parallelize << " " << onTrack << " " << turnAnticipation1 << " " << turnAnticipation2 << std::endl;

	XAxis = parallelize + onTrack + turnAnticipation1 + turnAnticipation2;
	if (XAxis > 0) XAxis = 1;
	else if (XAxis < 0) XAxis = -1;
	else XAxis = 0;
}

void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
												   btRigidBody* body1,
		btRigidBody* body2,

						const btMatrix3x3& world2A,
						const btMatrix3x3& world2B,
						
						const btVector3& invInertiaADiag,
						const btScalar invMassA,
						const btVector3& linvelA,const btVector3& angvelA,
						const btVector3& rel_posA1,
						const btVector3& invInertiaBDiag,
						const btScalar invMassB,
						const btVector3& linvelB,const btVector3& angvelB,
						const btVector3& rel_posA2,

					  btScalar depthA, const btVector3& normalA, 
					  const btVector3& rel_posB1,const btVector3& rel_posB2,
					  btScalar depthB, const btVector3& normalB, 
					  btScalar& imp0,btScalar& imp1)
{
	(void)linvelA;
	(void)linvelB;
	(void)angvelB;
	(void)angvelA;



	imp0 = btScalar(0.);
	imp1 = btScalar(0.);

	btScalar len = btFabs(normalA.length()) - btScalar(1.);
	if (btFabs(len) >= SIMD_EPSILON)
		return;

	btAssert(len < SIMD_EPSILON);


	//this jacobian entry could be re-used for all iterations
	btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
		invInertiaBDiag,invMassB);
	btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
		invInertiaBDiag,invMassB);
	
	//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
	//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);

	const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
	const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));

//	btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
	btScalar massTerm = btScalar(1.) / (invMassA + invMassB);


	// calculate rhs (or error) terms
	const btScalar dv0 = depthA  * m_tau * massTerm - vel0 * m_damping;
	const btScalar dv1 = depthB  * m_tau * massTerm - vel1 * m_damping;


	// dC/dv * dv = -C
	
	// jacobian * impulse = -error
	//

	//impulse = jacobianInverse * -error

	// inverting 2x2 symmetric system (offdiagonal are equal!)
	// 


	btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
	btScalar	invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
	
	//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
	//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;

	imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
	imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;

	//[a b]								  [d -c]
	//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)

	//[jA nD] * [imp0] = [dv0]
	//[nD jB]   [imp1]   [dv1]

}

void collisionWithBound(btVector3 &vel, btVector3 &pos, const btVector3 &normal, const btScalar &penetrationDist)
{
    if( penetrationDist < btScalar(0.0) )
	{
        
        pos = pos - normal * penetrationDist;
        
        vel = vel - (1 + Constant.m_boundaryRestitution * (-penetrationDist) / (Constant.m_timeStep * vel.length())) * vel.dot(normal) * normal;
	}
}