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

void CharacterController::computeNewVelocity(btScalar dt, btVector3& velocity) {
    if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) {
        velocity = btVector3(0.0f, 0.0f, 0.0f);
    }

    // measure velocity changes and their weights
    std::vector<btVector3> velocities;
    velocities.reserve(_motors.size());
    std::vector<btScalar> weights;
    weights.reserve(_motors.size());
    for (int i = 0; i < (int)_motors.size(); ++i) {
        applyMotor(i, dt, velocity, velocities, weights);
    }
    assert(velocities.size() == weights.size());

    // blend velocity changes according to relative weights
    btScalar totalWeight = 0.0f;
    for (size_t i = 0; i < weights.size(); ++i) {
        totalWeight += weights[i];
    }
    if (totalWeight > 0.0f) {
        velocity = btVector3(0.0f, 0.0f, 0.0f);
        for (size_t i = 0; i < velocities.size(); ++i) {
            velocity += (weights[i] / totalWeight) * velocities[i];
        }
    }
    if (velocity.length2() < MIN_TARGET_SPEED_SQUARED) {
        velocity = btVector3(0.0f, 0.0f, 0.0f);
    }

    // 'thrust' is applied at the very end
    velocity += dt * _linearAcceleration;
    _targetVelocity = velocity;
}

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 density (poly6)
 * @inproceedings{muller2003particle,
 *  title={Particle-based fluid simulation for interactive applications},
 *  author={M{\"u}ller, Matthias and Charypar, David and Gross, Markus},
 *  booktitle={Proceedings of the 2003 ACM SIGGRAPH/Eurographics symposium on Computer animation},
 *  pages={154--159},
 *  year={2003},
 *  organization={Eurographics Association}
 * }
 */
inline float VRFluids::kernel_poly6(btVector3 v, float h) {
    float r2 = v.length2();
    float h2 = h*h;
    if ( r2 <= h2 ) {
        float diff = h2 - r2;
        return (315.0 / (64.0*Pi)) * (1/pow(h,9)) * diff*diff*diff;
    } else {
        return 0.0;
    }
}

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

bool EpaFace::Initialize()
{
	assert( m_pHalfEdge && "Must setup half-edge first!" );

	CollectVertices( m_pVertices );
	
	const btVector3 e0 = m_pVertices[ 1 ]->m_point - m_pVertices[ 0 ]->m_point;
	const btVector3 e1 = m_pVertices[ 2 ]->m_point - m_pVertices[ 0 ]->m_point;

	const btScalar e0Sqrd = e0.length2();
	const btScalar e1Sqrd = e1.length2();
	const btScalar e0e1   = e0.dot( e1 );

	m_determinant = e0Sqrd * e1Sqrd - e0e1 * e0e1;

	const btScalar e0v0 = e0.dot( m_pVertices[ 0 ]->m_point );
	const btScalar e1v0 = e1.dot( m_pVertices[ 0 ]->m_point );

	m_lambdas[ 0 ] = e0e1 * e1v0 - e1Sqrd * e0v0;
	m_lambdas[ 1 ] = e0e1 * e0v0 - e0Sqrd * e1v0;

	if ( IsAffinelyDependent() )
	{
		return false;
	}

	CalcClosestPoint();

#ifdef EPA_POLYHEDRON_USE_PLANES
	if ( !CalculatePlane() )
	{
		return false;
	}
#endif

	return true;
}

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