Example #1
0
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;
}
Example #2
0
void btHeightfieldTerrainShape::quantizeWithClamp(int* out, const btVector3& point,int /*isMax*/) const
{
    btVector3 clampedPoint(point);
    clampedPoint.setMax(m_localAabbMin);
    clampedPoint.setMin(m_localAabbMax);

    btVector3 v = (clampedPoint);// - m_bvhAabbMin) * m_bvhQuantization;

    //TODO: optimization: check out how to removed this btFabs

    out[0] = (int)(v.getX() + v.getX() / btFabs(v.getX())* btScalar(0.5) );
    out[1] = (int)(v.getY() + v.getY() / btFabs(v.getY())* btScalar(0.5) );
    out[2] = (int)(v.getZ() + v.getZ() / btFabs(v.getZ())* btScalar(0.5) );

}
Example #3
0
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;
}
Example #4
0
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];
		}
Example #6
0
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;
}
Example #7
0
btVector3  ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
{
	btVector3 N1 = p0.normal;
	btVector3 N2 = p1.normal;
	btVector3 N3 = p2.normal;

	btVector3 n2n3; n2n3 = N2.cross(N3);
	btVector3 n3n1; n3n1 = N3.cross(N1);
	btVector3 n1n2; n1n2 = N1.cross(N2);

	btScalar quotient = (N1.dot(n2n3));

	btAssert(btFabs(quotient) > btScalar(0.000001));
	
	quotient = btScalar(-1.) / quotient;
	n2n3 *= p0.dist;
	n3n1 *= p1.dist;
	n1n2 *= p2.dist;
	btVector3 potentialVertex = n2n3;
	potentialVertex += n3n1;
	potentialVertex += n1n2;
	potentialVertex *= quotient;

	btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
	return result;

}
Example #8
0
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
}
Example #11
0
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;
}
Example #12
0
 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;
     }
 }
Example #13
0
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;
		}
	}
}
Example #14
0
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;
		}
	}
}
Example #16
0
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);
                            }
                        }
                    }
                }
            }
        }
    }
}
		virtual void internalProcessTriangleIndex(btVector3* triangle, int partId, int triangleIndex)
		{
			(void)triangleIndex;
			(void)partId;
			if (first)
			{
				ref = triangle[0];
				first = false;
			}
			else
			{
				btScalar vol = btFabs((triangle[0] - ref).triple(triangle[1] - ref, triangle[2] - ref));
				sum += (btScalar(0.25) * vol) * ((triangle[0] + triangle[1] + triangle[2] + ref));
				volume += vol;
			}
		}
Example #18
0
//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;
}
Example #19
0
/// Changes a btManifoldPoint collision normal to the normal from the mesh.
void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,const btCollisionObjectWrapper* colObj1Wrap, int partId0, int index0, int normalAdjustFlags)
{
	//btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE);
	if (colObj0Wrap->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
		return;

	btBvhTriangleMeshShape* trimesh = 0;
	
	if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
	{
		trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape();
	}
	else
	{
		if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
		{
			trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape();
		}
	}
	if (trimesh==0)
		return;

	btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
	if (!triangleInfoMapPtr)
		return;

	int hash = btGetHash(partId0,index0);


	btTriangleInfo* info = triangleInfoMapPtr->find(hash);
	if (!info)
		return;

	btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f;
	
	const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0Wrap->getCollisionShape());
	btVector3 v0,v1,v2;
	tri_shape->getVertex(0,v0);
	tri_shape->getVertex(1,v1);
	tri_shape->getVertex(2,v2);

	//btVector3 center = (v0+v1+v2)*btScalar(1./3.);

	btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0);
	btVector3 tri_normal;
	tri_shape->calcNormal(tri_normal);

	//btScalar dot = tri_normal.dot(cp.m_normalWorldOnB);
	btVector3 nearest;
	btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest);

	btVector3 contact = cp.m_localPointB;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
	const btTransform& tr = colObj0->getWorldTransform();
	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



	bool isNearEdge = false;

	int numConcaveEdgeHits = 0;
	int numConvexEdgeHits = 0;

	btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
	localContactNormalOnB.normalize();//is this necessary?
	
	// Get closest edge
	int      bestedge=-1;
	btScalar    disttobestedge=BT_LARGE_FLOAT;
	//
	// Edge 0 -> 1
	if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
	{	
	   btVector3 nearest;
	   btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
	   btScalar     len=(contact-nearest).length();
	   //
	   if( len < disttobestedge )
	   {
	      bestedge=0;
	      disttobestedge=len;
      }	      
   }	   
	// Edge 1 -> 2
	if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
	{	
	   btVector3 nearest;
	   btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
	   btScalar     len=(contact-nearest).length();
	   //
	   if( len < disttobestedge )
	   {
	      bestedge=1;
	      disttobestedge=len;
      }	      
   }	   
	// Edge 2 -> 0
	if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
	{	
	   btVector3 nearest;
	   btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
	   btScalar     len=(contact-nearest).length();
	   //
	   if( len < disttobestedge )
	   {
	      bestedge=2;
	      disttobestedge=len;
      }	      
   }   	      	
	
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
   btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);
   btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
#endif   
	if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
	{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif
		btScalar len = (contact-nearest).length();
		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
		if( bestedge==0 )
		{
			btVector3 edge(v0-v1);
			isNearEdge = true;

			if (info->m_edgeV0V1Angle==btScalar(0))
			{
				numConcaveEdgeHits++;
			} else
			{

				bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX);
				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

				btVector3 nA = swapFactor * tri_normal;

				btQuaternion orn(edge,info->m_edgeV0V1Angle);
				btVector3 computedNormalB = quatRotate(orn,tri_normal);
				if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB)
					computedNormalB*=-1;
				btVector3 nB = swapFactor*computedNormalB;

				btScalar	NdotA = localContactNormalOnB.dot(nA);
				btScalar	NdotB = localContactNormalOnB.dot(nB);
				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

#ifdef DEBUG_INTERNAL_EDGE
				{
					
					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
				}
#endif //DEBUG_INTERNAL_EDGE


				if (backFacingNormal)
				{
					numConcaveEdgeHits++;
				}
				else
				{
					numConvexEdgeHits++;
					btVector3 clampedLocalNormal;
					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal);
					if (isClamped)
					{
						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
						{
							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
							cp.m_normalWorldOnB = newNormal;
							// Reproject collision point along normal. (what about cp.m_distance1?)
							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
							
						}
					}
				}
			}
		}
	}

	btNearestPointInLineSegment(contact,v1,v2,nearest);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
   btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
#endif   

	if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
	{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW



		btScalar len = (contact-nearest).length();
		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
		if( bestedge==1 )
		{
			isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

			btVector3 edge(v1-v2);

			isNearEdge = true;

			if (info->m_edgeV1V2Angle == btScalar(0))
			{
				numConcaveEdgeHits++;
			} else
			{
				bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0;
				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

				btVector3 nA = swapFactor * tri_normal;
				
				btQuaternion orn(edge,info->m_edgeV1V2Angle);
				btVector3 computedNormalB = quatRotate(orn,tri_normal);
				if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB)
					computedNormalB*=-1;
				btVector3 nB = swapFactor*computedNormalB;

#ifdef DEBUG_INTERNAL_EDGE
				{
					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
				}
#endif //DEBUG_INTERNAL_EDGE


				btScalar	NdotA = localContactNormalOnB.dot(nA);
				btScalar	NdotB = localContactNormalOnB.dot(nB);
				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

				if (backFacingNormal)
				{
					numConcaveEdgeHits++;
				}
				else
				{
					numConvexEdgeHits++;
					btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
					btVector3 clampedLocalNormal;
					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal);
					if (isClamped)
					{
						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
						{
							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
							cp.m_normalWorldOnB = newNormal;
							// Reproject collision point along normal.
							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
						}
					}
				}
			}
		}
	}

	btNearestPointInLineSegment(contact,v2,v0,nearest);
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
	btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
   btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
#endif   

	if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
	{

#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
		btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

		btScalar len = (contact-nearest).length();
		if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
		if( bestedge==2 )
		{
			isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
			btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

			btVector3 edge(v2-v0);

			if (info->m_edgeV2V0Angle==btScalar(0))
			{
				numConcaveEdgeHits++;
			} else
			{

				bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0;
				btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1);
	#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
				btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white);
	#endif //BT_INTERNAL_EDGE_DEBUG_DRAW

				btVector3 nA = swapFactor * tri_normal;
				btQuaternion orn(edge,info->m_edgeV2V0Angle);
				btVector3 computedNormalB = quatRotate(orn,tri_normal);
				if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB)
					computedNormalB*=-1;
				btVector3 nB = swapFactor*computedNormalB;

#ifdef DEBUG_INTERNAL_EDGE
				{
					btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red);
				}
#endif //DEBUG_INTERNAL_EDGE

				btScalar	NdotA = localContactNormalOnB.dot(nA);
				btScalar	NdotB = localContactNormalOnB.dot(nB);
				bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon);

				if (backFacingNormal)
				{
					numConcaveEdgeHits++;
				}
				else
				{
					numConvexEdgeHits++;
					//				printf("hitting convex edge\n");


					btVector3 localContactNormalOnB = colObj0Wrap->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
					btVector3 clampedLocalNormal;
					bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal);
					if (isClamped)
					{
						if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0))
						{
							btVector3 newNormal = colObj0Wrap->getWorldTransform().getBasis() * clampedLocalNormal;
							//					cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB);
							cp.m_normalWorldOnB = newNormal;
							// Reproject collision point along normal.
							cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
							cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
						}
					}
				} 
			}
			

		}
	}

#ifdef DEBUG_INTERNAL_EDGE
	{
		btVector3 color(0,1,1);
		btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color);
	}
#endif //DEBUG_INTERNAL_EDGE

	if (isNearEdge)
	{

		if (numConcaveEdgeHits>0)
		{
			if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0)
			{
				//fix tri_normal so it pointing the same direction as the current local contact normal
				if (tri_normal.dot(localContactNormalOnB) < 0)
				{
					tri_normal *= -1;
				}
				cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis()*tri_normal;
			} else
			{
				btVector3 newNormal = tri_normal *frontFacing;
				//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
				btScalar d = newNormal.dot(localContactNormalOnB) ;
				if (d< 0)
				{
					return;
				}
				//modify the normal to be the triangle normal (or backfacing normal)
				cp.m_normalWorldOnB = colObj0Wrap->getWorldTransform().getBasis() *newNormal;
			}
						
			// Reproject collision point along normal.
			cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
			cp.m_localPointB = colObj0Wrap->getWorldTransform().invXform(cp.m_positionWorldOnB);
		}
	}
}
Example #20
0
void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
{
	const btTransform& trA = getCalculatedTransformA();
	const btTransform& trB = getCalculatedTransformB();
	
	btAssert(!m_useSolveConstraintObsolete);
	int i, s = info->rowskip;
	
	btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
	
	// difference between frames in WCS
	btVector3 ofs = trB.getOrigin() - trA.getOrigin();
	// now get weight factors depending on masses
	btScalar miA = rbAinvMass;
	btScalar miB = rbBinvMass;
	bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
	btScalar miS = miA + miB;
	btScalar factA, factB;
	if(miS > btScalar(0.f))
	{
		factA = miB / miS;
	}
	else 
	{
		factA = btScalar(0.5f);
	}
	factB = btScalar(1.0f) - factA;
	btVector3 ax1, p, q;
	btVector3 ax1A = trA.getBasis().getColumn(0);
	btVector3 ax1B = trB.getBasis().getColumn(0);
	if(m_useOffsetForConstraintFrame)
	{
		// get the desired direction of slider axis
		// as weighted sum of X-orthos of frameA and frameB in WCS
		ax1 = ax1A * factA + ax1B * factB;
		ax1.normalize();
		// construct two orthos to slider axis
		btPlaneSpace1 (ax1, p, q);
	}
	else
	{ // old way - use frameA
		ax1 = trA.getBasis().getColumn(0);
		// get 2 orthos to slider axis (Y, Z)
		p = trA.getBasis().getColumn(1);
		q = trA.getBasis().getColumn(2);
	}
	// make rotations around these orthos equal
	// the slider axis should be the only unconstrained
	// rotational axis, the angular velocity of the two bodies perpendicular to
	// the slider axis should be equal. thus the constraint equations are
	//    p*w1 - p*w2 = 0
	//    q*w1 - q*w2 = 0
	// where p and q are unit vectors normal to the slider axis, and w1 and w2
	// are the angular velocity vectors of the two bodies.
	info->m_J1angularAxis[0] = p[0];
	info->m_J1angularAxis[1] = p[1];
	info->m_J1angularAxis[2] = p[2];
	info->m_J1angularAxis[s+0] = q[0];
	info->m_J1angularAxis[s+1] = q[1];
	info->m_J1angularAxis[s+2] = q[2];

	info->m_J2angularAxis[0] = -p[0];
	info->m_J2angularAxis[1] = -p[1];
	info->m_J2angularAxis[2] = -p[2];
	info->m_J2angularAxis[s+0] = -q[0];
	info->m_J2angularAxis[s+1] = -q[1];
	info->m_J2angularAxis[s+2] = -q[2];
	// compute the right hand side of the constraint equation. set relative
	// body velocities along p and q to bring the slider back into alignment.
	// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
	// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
	// if "theta" is the angle between ax1 and ax2, we need an angular velocity
	// along u to cover angle erp*theta in one step :
	//   |angular_velocity| = angle/time = erp*theta / stepsize
	//                      = (erp*fps) * theta
	//    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
	//                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
	// ...as ax1 and ax2 are unit length. if theta is smallish,
	// theta ~= sin(theta), so
	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
	// ax1 x ax2 is in the plane space of ax1, so we project the angular
	// velocity to p and q to find the right hand side.
//	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
	btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
	btScalar k = info->fps * currERP;

	btVector3 u = ax1A.cross(ax1B);
	info->m_constraintError[0] = k * u.dot(p);
	info->m_constraintError[s] = k * u.dot(q);
	if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
	{
		info->cfm[0] = m_cfmOrthoAng;
		info->cfm[s] = m_cfmOrthoAng;
	}

	int nrow = 1; // last filled row
	int srow;
	btScalar limit_err;
	int limit;
	int powered;

	// next two rows. 
	// we want: velA + wA x relA == velB + wB x relB ... but this would
	// result in three equations, so we project along two orthos to the slider axis

	btTransform bodyA_trans = transA;
	btTransform bodyB_trans = transB;
	nrow++;
	int s2 = nrow * s;
	nrow++;
	int s3 = nrow * s;
	btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
	if(m_useOffsetForConstraintFrame)
	{
		// get vector from bodyB to frameB in WCS
		relB = trB.getOrigin() - bodyB_trans.getOrigin();
		// get its projection to slider axis
		btVector3 projB = ax1 * relB.dot(ax1);
		// get vector directed from bodyB to slider axis (and orthogonal to it)
		btVector3 orthoB = relB - projB;
		// same for bodyA
		relA = trA.getOrigin() - bodyA_trans.getOrigin();
		btVector3 projA = ax1 * relA.dot(ax1);
		btVector3 orthoA = relA - projA;
		// get desired offset between frames A and B along slider axis
		btScalar sliderOffs = m_linPos - m_depth[0];
		// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
		btVector3 totalDist = projA + ax1 * sliderOffs - projB;
		// get offset vectors relA and relB
		relA = orthoA + totalDist * factA;
		relB = orthoB - totalDist * factB;
		// now choose average ortho to slider axis
		p = orthoB * factA + orthoA * factB;
		btScalar len2 = p.length2();
		if(len2 > SIMD_EPSILON)
		{
			p /= btSqrt(len2);
		}
		else
		{
			p = trA.getBasis().getColumn(1);
		}
		// make one more ortho
		q = ax1.cross(p);
		// fill two rows
		tmpA = relA.cross(p);
		tmpB = relB.cross(p);
		for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
		for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
		tmpA = relA.cross(q);
		tmpB = relB.cross(q);
		if(hasStaticBody && getSolveAngLimit())
		{ // to make constraint between static and dynamic objects more rigid
			// remove wA (or wB) from equation if angular limit is hit
			tmpB *= factB;
			tmpA *= factA;
		}
		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
		for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
		for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
	}
	else
	{	// old way - maybe incorrect if bodies are not on the slider axis
		// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
		c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
		btVector3 tmp = c.cross(p);
		for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
		for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
		tmp = c.cross(q);
		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];

		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
		for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i];
		for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i];
	}
	// compute two elements of right hand side

	//	k = info->fps * info->erp * getSoftnessOrthoLin();
	currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
	k = info->fps * currERP;

	btScalar rhs = k * p.dot(ofs);
	info->m_constraintError[s2] = rhs;
	rhs = k * q.dot(ofs);
	info->m_constraintError[s3] = rhs;
	if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
	{
		info->cfm[s2] = m_cfmOrthoLin;
		info->cfm[s3] = m_cfmOrthoLin;
	}


	// check linear limits
	limit_err = btScalar(0.0);
	limit = 0;
	if(getSolveLinLimit())
	{
		limit_err = getLinDepth() *  signFact;
		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
	}
	powered = 0;
	if(getPoweredLinMotor())
	{
		powered = 1;
	}
	// if the slider has joint limits or motor, add in the extra row
	if (limit || powered) 
	{
		nrow++;
		srow = nrow * info->rowskip;
		info->m_J1linearAxis[srow+0] = ax1[0];
		info->m_J1linearAxis[srow+1] = ax1[1];
		info->m_J1linearAxis[srow+2] = ax1[2];
		info->m_J2linearAxis[srow+0] = -ax1[0];
		info->m_J2linearAxis[srow+1] = -ax1[1];
		info->m_J2linearAxis[srow+2] = -ax1[2];
		// linear torque decoupling step:
		//
		// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
		// do not create a torque couple. in other words, the points that the
		// constraint force is applied at must lie along the same ax1 axis.
		// a torque couple will result in limited slider-jointed free
		// bodies from gaining angular momentum.
		if(m_useOffsetForConstraintFrame)
		{
			// this is needed only when bodyA and bodyB are both dynamic.
			if(!hasStaticBody)
			{
				tmpA = relA.cross(ax1);
				tmpB = relB.cross(ax1);
				info->m_J1angularAxis[srow+0] = tmpA[0];
				info->m_J1angularAxis[srow+1] = tmpA[1];
				info->m_J1angularAxis[srow+2] = tmpA[2];
				info->m_J2angularAxis[srow+0] = -tmpB[0];
				info->m_J2angularAxis[srow+1] = -tmpB[1];
				info->m_J2angularAxis[srow+2] = -tmpB[2];
			}
		}
		else
		{ // The old way. May be incorrect if bodies are not on the slider axis
			btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
			ltd = c.cross(ax1);
			info->m_J1angularAxis[srow+0] = factA*ltd[0];
			info->m_J1angularAxis[srow+1] = factA*ltd[1];
			info->m_J1angularAxis[srow+2] = factA*ltd[2];
			info->m_J2angularAxis[srow+0] = factB*ltd[0];
			info->m_J2angularAxis[srow+1] = factB*ltd[1];
			info->m_J2angularAxis[srow+2] = factB*ltd[2];
		}
		// right-hand part
		btScalar lostop = getLowerLinLimit();
		btScalar histop = getUpperLinLimit();
		if(limit && (lostop == histop))
		{  // the joint motor is ineffective
			powered = 0;
		}
		info->m_constraintError[srow] = 0.;
		info->m_lowerLimit[srow] = 0.;
		info->m_upperLimit[srow] = 0.;
		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
		if(powered)
		{
			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
			{
				info->cfm[srow] = m_cfmDirLin;
			}
			btScalar tag_vel = getTargetLinMotorVelocity();
			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
			info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
			info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
			info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
		}
		if(limit)
		{
			k = info->fps * currERP;
			info->m_constraintError[srow] += k * limit_err;
			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
			{
				info->cfm[srow] = m_cfmLimLin;
			}
			if(lostop == histop) 
			{	// limited low and high simultaneously
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else if(limit == 1) 
			{ // low limit
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = 0;
			}
			else 
			{ // high limit
				info->m_lowerLimit[srow] = 0;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
			if(bounce > btScalar(0.0))
			{
				btScalar vel = linVelA.dot(ax1);
				vel -= linVelB.dot(ax1);
				vel *= signFact;
				// only apply bounce if the velocity is incoming, and if the
				// resulting c[] exceeds what we already have.
				if(limit == 1)
				{	// low limit
					if(vel < 0)
					{
						btScalar newc = -bounce * vel;
						if (newc > info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
				else
				{ // high limit - all those computations are reversed
					if(vel > 0)
					{
						btScalar newc = -bounce * vel;
						if(newc < info->m_constraintError[srow]) 
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
			}
			info->m_constraintError[srow] *= getSoftnessLimLin();
		} // if(limit)
	} // if linear limit
	// check angular limits
	limit_err = btScalar(0.0);
	limit = 0;
	if(getSolveAngLimit())
	{
		limit_err = getAngDepth();
		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
	}
	// if the slider has joint limits, add in the extra row
	powered = 0;
	if(getPoweredAngMotor())
	{
		powered = 1;
	}
	if(limit || powered) 
	{
		nrow++;
		srow = nrow * info->rowskip;
		info->m_J1angularAxis[srow+0] = ax1[0];
		info->m_J1angularAxis[srow+1] = ax1[1];
		info->m_J1angularAxis[srow+2] = ax1[2];

		info->m_J2angularAxis[srow+0] = -ax1[0];
		info->m_J2angularAxis[srow+1] = -ax1[1];
		info->m_J2angularAxis[srow+2] = -ax1[2];

		btScalar lostop = getLowerAngLimit();
		btScalar histop = getUpperAngLimit();
		if(limit && (lostop == histop))
		{  // the joint motor is ineffective
			powered = 0;
		}
		currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
		if(powered)
		{
			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
			{
				info->cfm[srow] = m_cfmDirAng;
			}
			btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
			info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
			info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
			info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
		}
		if(limit)
		{
			k = info->fps * currERP;
			info->m_constraintError[srow] += k * limit_err;
			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
			{
				info->cfm[srow] = m_cfmLimAng;
			}
			if(lostop == histop) 
			{
				// limited low and high simultaneously
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else if(limit == 1) 
			{ // low limit
				info->m_lowerLimit[srow] = 0;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else 
			{ // high limit
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = 0;
			}
			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
			if(bounce > btScalar(0.0))
			{
				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
				vel -= m_rbB.getAngularVelocity().dot(ax1);
				// only apply bounce if the velocity is incoming, and if the
				// resulting c[] exceeds what we already have.
				if(limit == 1)
				{	// low limit
					if(vel < 0)
					{
						btScalar newc = -bounce * vel;
						if(newc > info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
				else
				{	// high limit - all those computations are reversed
					if(vel > 0)
					{
						btScalar newc = -bounce * vel;
						if(newc < info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
			}
			info->m_constraintError[srow] *= getSoftnessLimAng();
		} // if(limit)
	} // if angular limit or powered
}
void btConeTwistConstraint::calcAngleInfo()
{
	m_swingCorrection = btScalar(0.);
	m_twistLimitSign = btScalar(0.);
	m_solveTwistLimit = false;
	m_solveSwingLimit = false;

	btVector3 b1Axis1,b1Axis2,b1Axis3;
	btVector3 b2Axis1,b2Axis2;

	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);

	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);

	btScalar swx=btScalar(0.),swy = btScalar(0.);
	btScalar thresh = btScalar(10.);
	btScalar fact;

	// Get Frame into world space
	if (m_swingSpan1 >= btScalar(0.05f))
	{
		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
		swx = b2Axis1.dot(b1Axis1);
		swy = b2Axis1.dot(b1Axis2);
		swing1  = btAtan2Fast(swy, swx);
		fact = (swy*swy + swx*swx) * thresh * thresh;
		fact = fact / (fact + btScalar(1.0));
		swing1 *= fact; 
	}

	if (m_swingSpan2 >= btScalar(0.05f))
	{
		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
		swx = b2Axis1.dot(b1Axis1);
		swy = b2Axis1.dot(b1Axis3);
		swing2  = btAtan2Fast(swy, swx);
		fact = (swy*swy + swx*swx) * thresh * thresh;
		fact = fact / (fact + btScalar(1.0));
		swing2 *= fact; 
	}

	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
	btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;

	if (EllipseAngle > 1.0f)
	{
		m_swingCorrection = EllipseAngle-1.0f;
		m_solveSwingLimit = true;
		// Calculate necessary axis & factors
		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
		m_swingAxis.normalize();
		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
		m_swingAxis *= swingAxisSign;
	}

	// Twist limits
	if (m_twistSpan >= btScalar(0.))
	{
		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
		m_twistAngle = twist;

//		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
		if (twist <= -m_twistSpan*lockedFreeFactor)
		{
			m_twistCorrection = -(twist + m_twistSpan);
			m_solveTwistLimit = true;
			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
			m_twistAxis.normalize();
			m_twistAxis *= -1.0f;
		}
		else if (twist >  m_twistSpan*lockedFreeFactor)
		{
			m_twistCorrection = (twist - m_twistSpan);
			m_solveTwistLimit = true;
			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
			m_twistAxis.normalize();
		}
	}
}
btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
														btMultiBodyJacobianData& data,
														btScalar* jacOrgA,btScalar* jacOrgB,
														const btContactSolverInfo& infoGlobal,
														btScalar desiredVelocity,
														btScalar lowerLimit,
														btScalar upperLimit)
{
			
	
	
	constraintRow.m_multiBodyA = m_bodyA;
	constraintRow.m_multiBodyB = m_bodyB;

	btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
	btMultiBody* multiBodyB = constraintRow.m_multiBodyB;

	if (multiBodyA)
	{
		
		const int ndofA  = multiBodyA->getNumLinks() + 6;

		constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();

		if (constraintRow.m_deltaVelAindex <0)
		{
			constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
			multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
		} else
		{
			btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
		}

		constraintRow.m_jacAindex = data.m_jacobians.size();
		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
		for (int i=0;i<ndofA;i++)
			data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
		
		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
	} 

	if (multiBodyB)
	{
		const int ndofB  = multiBodyB->getNumLinks() + 6;

		constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
		if (constraintRow.m_deltaVelBindex <0)
		{
			constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
			multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
		}

		constraintRow.m_jacBindex = data.m_jacobians.size();
		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);

		for (int i=0;i<ndofB;i++)
			data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];

		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
	} 
	{
						
		btVector3 vec;
		btScalar denom0 = 0.f;
		btScalar denom1 = 0.f;
		btScalar* jacB = 0;
		btScalar* jacA = 0;
		btScalar* lambdaA =0;
		btScalar* lambdaB =0;
		int ndofA  = 0;
		if (multiBodyA)
		{
			ndofA  = multiBodyA->getNumLinks() + 6;
			jacA = &data.m_jacobians[constraintRow.m_jacAindex];
			lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
			for (int i = 0; i < ndofA; ++i)
			{
				btScalar j = jacA[i] ;
				btScalar l =lambdaA[i];
				denom0 += j*l;
			}
		} 
		if (multiBodyB)
		{
			const int ndofB  = multiBodyB->getNumLinks() + 6;
			jacB = &data.m_jacobians[constraintRow.m_jacBindex];
			lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
			for (int i = 0; i < ndofB; ++i)
			{
				btScalar j = jacB[i] ;
				btScalar l =lambdaB[i];
				denom1 += j*l;
			}

		} 

		 if (multiBodyA && (multiBodyA==multiBodyB))
		 {
            // ndof1 == ndof2 in this case
            for (int i = 0; i < ndofA; ++i) 
			{
                denom1 += jacB[i] * lambdaA[i];
                denom1 += jacA[i] * lambdaB[i];
            }
        }

		 btScalar d = denom0+denom1;
		 if (btFabs(d)>SIMD_EPSILON)
		 {
			 
			 constraintRow.m_jacDiagABInv = 1.f/(d);
		 } else
		 {
			constraintRow.m_jacDiagABInv  = 1.f;
		 }
		
	}

	
	//compute rhs and remaining constraintRow fields

	


	btScalar rel_vel = 0.f;
	int ndofA  = 0;
	int ndofB  = 0;
	{

		btVector3 vel1,vel2;
		if (multiBodyA)
		{
			ndofA  = multiBodyA->getNumLinks() + 6;
			btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
			for (int i = 0; i < ndofA ; ++i) 
				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
		} 
		if (multiBodyB)
		{
			ndofB  = multiBodyB->getNumLinks() + 6;
			btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
			for (int i = 0; i < ndofB ; ++i) 
				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];

		}

		constraintRow.m_friction = 0.f;

		constraintRow.m_appliedImpulse = 0.f;
		constraintRow.m_appliedPushImpulse = 0.f;
		
		btScalar	velocityError =  desiredVelocity - rel_vel;// * damping;

		btScalar erp = infoGlobal.m_erp2;

		btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;

		if (!infoGlobal.m_splitImpulse)
		{
			//combine position and velocity into rhs
			constraintRow.m_rhs = velocityImpulse;
			constraintRow.m_rhsPenetration = 0.f;

		} else
		{
			//split position and velocity into rhs and m_rhsPenetration
			constraintRow.m_rhs = velocityImpulse;
			constraintRow.m_rhsPenetration = 0.f;
		}


		constraintRow.m_cfm = 0.f;
		constraintRow.m_lowerLimit = lowerLimit;
		constraintRow.m_upperLimit = upperLimit;

	}
	return rel_vel;
}
Example #23
0
void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
{
    int i;
    // linear
    btVector3 velA;
	bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
    btVector3 velB;
	bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
    btVector3 vel = velA - velB;
	for(i = 0; i < 3; i++)
    {
		const btVector3& normal = m_jacLin[i].m_linearJointAxis;
		btScalar rel_vel = normal.dot(vel);
		// calculate positional error
		btScalar depth = m_depth[i];
		// get parameters
		btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
		btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
		btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
		// calcutate and apply impulse
		btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
		btVector3 impulse_vector = normal * normalImpulse;
		
		//rbA.applyImpulse( impulse_vector, m_relPosA);
		//rbB.applyImpulse(-impulse_vector, m_relPosB);
		{
			btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
			btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
			bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
			bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
		}



		if(m_poweredLinMotor && (!i))
		{ // apply linear motor
			if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
			{
				btScalar desiredMotorVel = m_targetLinMotorVelocity;
				btScalar motor_relvel = desiredMotorVel + rel_vel;
				normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
				// clamp accumulated impulse
				btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
				if(new_acc  > m_maxLinMotorForce)
				{
					new_acc = m_maxLinMotorForce;
				}
				btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
				if(normalImpulse < btScalar(0.0))
				{
					normalImpulse = -del;
				}
				else
				{
					normalImpulse = del;
				}
				m_accumulatedLinMotorImpulse = new_acc;
				// apply clamped impulse
				impulse_vector = normal * normalImpulse;
				//rbA.applyImpulse( impulse_vector, m_relPosA);
				//rbB.applyImpulse(-impulse_vector, m_relPosB);

				{
					btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
					btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
					bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
					bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
				}



			}
		}
    }
	// angular 
	// get axes in world space
	btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
	btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);

	btVector3 angVelA;
	bodyA.getAngularVelocity(angVelA);
	btVector3 angVelB;
	bodyB.getAngularVelocity(angVelB);

	btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
	btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);

	btVector3 angAorthog = angVelA - angVelAroundAxisA;
	btVector3 angBorthog = angVelB - angVelAroundAxisB;
	btVector3 velrelOrthog = angAorthog-angBorthog;
	//solve orthogonal angular velocity correction
	btScalar len = velrelOrthog.length();
	btScalar orthorImpulseMag = 0.f;

	if (len > btScalar(0.00001))
	{
		btVector3 normal = velrelOrthog.normalized();
		btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
		//velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
		orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
	}
	//solve angular positional correction
	btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
	btVector3 angularAxis = angularError;
	btScalar angularImpulseMag = 0;

	btScalar len2 = angularError.length();
	if (len2>btScalar(0.00001))
	{
		btVector3 normal2 = angularError.normalized();
		btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
		angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
		angularError *= angularImpulseMag;
	}
	// apply impulse
	//rbA.applyTorqueImpulse(-velrelOrthog+angularError);
	//rbB.applyTorqueImpulse(velrelOrthog-angularError);

	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);


	btScalar impulseMag;
	//solve angular limits
	if(m_solveAngLim)
	{
		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
		impulseMag *= m_kAngle * m_softnessLimAng;
	}
	else
	{
		impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
		impulseMag *= m_kAngle * m_softnessDirAng;
	}
	btVector3 impulse = axisA * impulseMag;
	//rbA.applyTorqueImpulse(impulse);
	//rbB.applyTorqueImpulse(-impulse);

	bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
	bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);



	//apply angular motor
	if(m_poweredAngMotor) 
	{
		if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
		{
			btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
			btScalar projRelVel = velrel.dot(axisA);

			btScalar desiredMotorVel = m_targetAngMotorVelocity;
			btScalar motor_relvel = desiredMotorVel - projRelVel;

			btScalar angImpulse = m_kAngle * motor_relvel;
			// clamp accumulated impulse
			btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
			if(new_acc  > m_maxAngMotorForce)
			{
				new_acc = m_maxAngMotorForce;
			}
			btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
			if(angImpulse < btScalar(0.0))
			{
				angImpulse = -del;
			}
			else
			{
				angImpulse = del;
			}
			m_accumulatedAngMotorImpulse = new_acc;
			// apply clamped impulse
			btVector3 motorImp = angImpulse * axisA;
			//rbA.applyTorqueImpulse(motorImp);
			//rbB.applyTorqueImpulse(-motorImp);

			bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
			bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
		}
	}
}
Example #24
0
void gim_contact_array::merge_contacts(
	const gim_contact_array & contacts, bool normal_contact_average)
{
	clear();

	if(contacts.size()==1)
	{
		push_back(contacts.back());
		return;
	}

	gim_array<GIM_RSORT_TOKEN> keycontacts(contacts.size());
	keycontacts.resize(contacts.size(),false);

	//fill key contacts

	GUINT i;

	for (i = 0;i<contacts.size() ;i++ )
	{
		keycontacts[i].m_key = contacts[i].calc_key_contact();
		keycontacts[i].m_value = i;
	}

	//sort keys
	gim_heap_sort(keycontacts.pointer(),keycontacts.size(),GIM_RSORT_TOKEN_COMPARATOR());

	// Merge contacts

	GUINT coincident_count=0;
	btVector3 coincident_normals[MAX_COINCIDENT];

	GUINT last_key = keycontacts[0].m_key;
	GUINT key = 0;

	push_back(contacts[keycontacts[0].m_value]);
	GIM_CONTACT * pcontact = &back();



	for( i=1;i<keycontacts.size();i++)
	{
	    key = keycontacts[i].m_key;
		const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];

		if(last_key ==  key)//same points
		{
			//merge contact
			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
			{
				*pcontact = *scontact;
                coincident_count = 0;
			}
			else if(normal_contact_average)
			{
				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
                {
                    if(coincident_count<MAX_COINCIDENT)
                    {
                    	coincident_normals[coincident_count] = scontact->m_normal;
                        coincident_count++;
                    }
                }
			}
		}
		else
		{//add new contact

		    if(normal_contact_average && coincident_count>0)
		    {
		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
		        coincident_count = 0;
		    }

		    push_back(*scontact);
		    pcontact = &back();
        }
		last_key = key;
	}
}
Example #25
0
void btSliderConstraint::getInfo2(btConstraintInfo2* info)
{
	btAssert(!m_useSolveConstraintObsolete);
	int i, s = info->rowskip;
	const btTransform& trA = getCalculatedTransformA();
	const btTransform& trB = getCalculatedTransformB();
	btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
	// make rotations around Y and Z equal
	// the slider axis should be the only unconstrained
	// rotational axis, the angular velocity of the two bodies perpendicular to
	// the slider axis should be equal. thus the constraint equations are
	//    p*w1 - p*w2 = 0
	//    q*w1 - q*w2 = 0
	// where p and q are unit vectors normal to the slider axis, and w1 and w2
	// are the angular velocity vectors of the two bodies.
	// get slider axis (X)
	btVector3 ax1 = trA.getBasis().getColumn(0);
	// get 2 orthos to slider axis (Y, Z)
	btVector3 p = trA.getBasis().getColumn(1);
	btVector3 q = trA.getBasis().getColumn(2);
	// set the two slider rows 
	info->m_J1angularAxis[0] = p[0];
	info->m_J1angularAxis[1] = p[1];
	info->m_J1angularAxis[2] = p[2];
	info->m_J1angularAxis[s+0] = q[0];
	info->m_J1angularAxis[s+1] = q[1];
	info->m_J1angularAxis[s+2] = q[2];

	info->m_J2angularAxis[0] = -p[0];
	info->m_J2angularAxis[1] = -p[1];
	info->m_J2angularAxis[2] = -p[2];
	info->m_J2angularAxis[s+0] = -q[0];
	info->m_J2angularAxis[s+1] = -q[1];
	info->m_J2angularAxis[s+2] = -q[2];
	// compute the right hand side of the constraint equation. set relative
	// body velocities along p and q to bring the slider back into alignment.
	// if ax1,ax2 are the unit length slider axes as computed from body1 and
	// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
	// if "theta" is the angle between ax1 and ax2, we need an angular velocity
	// along u to cover angle erp*theta in one step :
	//   |angular_velocity| = angle/time = erp*theta / stepsize
	//                      = (erp*fps) * theta
	//    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
	//                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
	// ...as ax1 and ax2 are unit length. if theta is smallish,
	// theta ~= sin(theta), so
	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
	// ax1 x ax2 is in the plane space of ax1, so we project the angular
	// velocity to p and q to find the right hand side.
	btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
    btVector3 ax2 = trB.getBasis().getColumn(0);
	btVector3 u = ax1.cross(ax2);
	info->m_constraintError[0] = k * u.dot(p);
	info->m_constraintError[s] = k * u.dot(q);
	// pull out pos and R for both bodies. also get the connection
	// vector c = pos2-pos1.
	// next two rows. we want: vel2 = vel1 + w1 x c ... but this would
	// result in three equations, so we project along the planespace vectors
	// so that sliding along the slider axis is disregarded. for symmetry we
	// also consider rotation around center of mass of two bodies (factA and factB).
	btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
	btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
	int s2 = 2 * s, s3 = 3 * s;
	btVector3 c;
	btScalar miA = m_rbA.getInvMass();
	btScalar miB = m_rbB.getInvMass();
	btScalar miS = miA + miB;
	btScalar factA, factB;
	if(miS > btScalar(0.f))
	{
		factA = miB / miS;
	}
	else 
	{
		factA = btScalar(0.5f);
	}
	if(factA > 0.99f) factA = 0.99f;
	if(factA < 0.01f) factA = 0.01f;
	factB = btScalar(1.0f) - factA;
	c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
	btVector3 tmp = c.cross(p);
	for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
	for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
	tmp = c.cross(q);
	for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
	for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];

	for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
	for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
	// compute two elements of right hand side. we want to align the offset
	// point (in body 2's frame) with the center of body 1.
	btVector3 ofs; // offset point in global coordinates
	ofs = trB.getOrigin() - trA.getOrigin();
	k = info->fps * info->erp * getSoftnessOrthoLin();
	info->m_constraintError[s2] = k * p.dot(ofs);
	info->m_constraintError[s3] = k * q.dot(ofs);
	int nrow = 3; // last filled row
	int srow;
	// check linear limits linear
	btScalar limit_err = btScalar(0.0);
	int limit = 0;
	if(getSolveLinLimit())
	{
		limit_err = getLinDepth() *  signFact;
		limit = (limit_err > btScalar(0.0)) ? 2 : 1;
	}
	int powered = 0;
	if(getPoweredLinMotor())
	{
		powered = 1;
	}
	// if the slider has joint limits or motor, add in the extra row
	if (limit || powered) 
	{
		nrow++;
		srow = nrow * info->rowskip;
		info->m_J1linearAxis[srow+0] = ax1[0];
		info->m_J1linearAxis[srow+1] = ax1[1];
		info->m_J1linearAxis[srow+2] = ax1[2];
		// linear torque decoupling step:
		//
		// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
		// do not create a torque couple. in other words, the points that the
		// constraint force is applied at must lie along the same ax1 axis.
		// a torque couple will result in limited slider-jointed free
		// bodies from gaining angular momentum.
		// the solution used here is to apply the constraint forces at the center of mass of the two bodies
		btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
//		c = btScalar(0.5) * c;
		ltd = c.cross(ax1);
		info->m_J1angularAxis[srow+0] = factA*ltd[0];
		info->m_J1angularAxis[srow+1] = factA*ltd[1];
		info->m_J1angularAxis[srow+2] = factA*ltd[2];
		info->m_J2angularAxis[srow+0] = factB*ltd[0];
		info->m_J2angularAxis[srow+1] = factB*ltd[1];
		info->m_J2angularAxis[srow+2] = factB*ltd[2];
		// right-hand part
		btScalar lostop = getLowerLinLimit();
		btScalar histop = getUpperLinLimit();
		if(limit && (lostop == histop))
		{  // the joint motor is ineffective
			powered = 0;
		}
		info->m_constraintError[srow] = 0.;
		info->m_lowerLimit[srow] = 0.;
		info->m_upperLimit[srow] = 0.;
		if(powered)
		{
            info->cfm[nrow] = btScalar(0.0); 
			btScalar tag_vel = getTargetLinMotorVelocity();
			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
//			info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
			info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
			info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
			info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
		}
		if(limit)
		{
			k = info->fps * info->erp;
			info->m_constraintError[srow] += k * limit_err;
			info->cfm[srow] = btScalar(0.0); // stop_cfm;
			if(lostop == histop) 
			{	// limited low and high simultaneously
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else if(limit == 1) 
			{ // low limit
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = 0;
			}
			else 
			{ // high limit
				info->m_lowerLimit[srow] = 0;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
			if(bounce > btScalar(0.0))
			{
				btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
				vel -= m_rbB.getLinearVelocity().dot(ax1);
				vel *= signFact;
				// only apply bounce if the velocity is incoming, and if the
				// resulting c[] exceeds what we already have.
				if(limit == 1)
				{	// low limit
					if(vel < 0)
					{
						btScalar newc = -bounce * vel;
						if (newc > info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
				else
				{ // high limit - all those computations are reversed
					if(vel > 0)
					{
						btScalar newc = -bounce * vel;
						if(newc < info->m_constraintError[srow]) 
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
			}
			info->m_constraintError[srow] *= getSoftnessLimLin();
		} // if(limit)
	} // if linear limit
	// check angular limits
	limit_err = btScalar(0.0);
	limit = 0;
	if(getSolveAngLimit())
	{
		limit_err = getAngDepth();
		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
	}
	// if the slider has joint limits, add in the extra row
	powered = 0;
	if(getPoweredAngMotor())
	{
		powered = 1;
	}
	if(limit || powered) 
	{
		nrow++;
		srow = nrow * info->rowskip;
		info->m_J1angularAxis[srow+0] = ax1[0];
		info->m_J1angularAxis[srow+1] = ax1[1];
		info->m_J1angularAxis[srow+2] = ax1[2];

		info->m_J2angularAxis[srow+0] = -ax1[0];
		info->m_J2angularAxis[srow+1] = -ax1[1];
		info->m_J2angularAxis[srow+2] = -ax1[2];

		btScalar lostop = getLowerAngLimit();
		btScalar histop = getUpperAngLimit();
		if(limit && (lostop == histop))
		{  // the joint motor is ineffective
			powered = 0;
		}
		if(powered)
		{
            info->cfm[srow] = btScalar(0.0); 
			btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
			info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
			info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
			info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
		}
		if(limit)
		{
			k = info->fps * info->erp;
			info->m_constraintError[srow] += k * limit_err;
			info->cfm[srow] = btScalar(0.0); // stop_cfm;
			if(lostop == histop) 
			{
				// limited low and high simultaneously
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else if(limit == 1) 
			{ // low limit
				info->m_lowerLimit[srow] = 0;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else 
			{ // high limit
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = 0;
			}
			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
			btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
			if(bounce > btScalar(0.0))
			{
				btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
				vel -= m_rbB.getAngularVelocity().dot(ax1);
				// only apply bounce if the velocity is incoming, and if the
				// resulting c[] exceeds what we already have.
				if(limit == 1)
				{	// low limit
					if(vel < 0)
					{
						btScalar newc = -bounce * vel;
						if(newc > info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
				else
				{	// high limit - all those computations are reversed
					if(vel > 0)
					{
						btScalar newc = -bounce * vel;
						if(newc < info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
			}
			info->m_constraintError[srow] *= getSoftnessLimAng();
		} // if(limit)
	} // if angular limit or powered
}
Example #26
0
void Vehicle::updateDynamics(btScalar dt)
{
	// differentials (constant, should happen at initialisation maybe?)
	for (int i = 0; i < differential.size(); ++i)
	{
		DifferentialJoint & djoint = diff_joint[i];
		djoint.shaft1 = &differential[i].getShaft();
		djoint.shaft2a = &differential[i].getShaft1();
		djoint.shaft2b = &differential[i].getShaft2();
		djoint.gearRatio = differential[i].getFinalDrive();
		djoint.init();

		ClutchJoint & cjoint = clutch_joint[i];
		cjoint.shaft1 = &differential[i].getShaft1();
		cjoint.shaft2 = &differential[i].getShaft2();
		cjoint.gearRatio = 1;
		// 1 way lsd, make it configurable?
		if (djoint.getVelocityDelta() > 0)
			cjoint.impulseLimit = differential[i].getAntiSlipTorque() * dt;
		else
			cjoint.impulseLimit = 0;
		cjoint.init();
	}

	// transmission and clutch
	int dcount = differential.size();
	int ccount = differential.size();
	//if (btFabs(transmission.getGearRatio()) > 0.0)
	{
		ClutchJoint & cjoint = clutch_joint[ccount];
		cjoint.shaft1 = &engine.getShaft();
		cjoint.shaft2 = &transmission.getShaft();
		cjoint.gearRatio = transmission.getGearRatio();
		cjoint.impulseLimit = transmission.getGearRatio() != 0 ? clutch.getTorque() * dt : 0;
		cjoint.init();
		ccount++;
	}

	// wheel displacement
	for (int i = 0; i < wheel.size(); ++i)
	{
		wheel[i].updateDisplacement(2 * wheel[i].getRadius());
	}

	// anti-roll bar approximation by adjusting suspension stiffness
	for (int i = 0; i < antiroll.size(); ++i)
	{
		// move this into antirollbar class ?

		// calculate anti-roll contributed stiffness
		btScalar kr = antiroll[i].stiffness;
		int i0 = antiroll[i].wheel0;
		int i1 = antiroll[i].wheel1;
		btScalar d0 = wheel[i0].suspension.getDisplacement();
		btScalar d1 = wheel[i1].suspension.getDisplacement();
		btScalar dr = d0 - d1;
		btScalar k0 = (d0 > 0) ? kr * dr / d0 : 0.0f;
		btScalar k1 = (d1 > 0) ? -kr * dr / d1 : 0.0f;

		// avoid negative stiffness
		if (wheel[i0].suspension.getStiffness() + k0 < 0) k0 = 0.0f;
		if (wheel[i1].suspension.getStiffness() + k1 < 0) k1 = 0.0f;

		wheel[i0].setAntiRollStiffness(k0);
		wheel[i1].setAntiRollStiffness(k1);
	}

	// wheel contacts
	int mcount = 0;
	int wcount = 0;
	abs_active = false;
	tcs_active = false;
	for (int i = 0; i < wheel.size(); ++i)
	{
		if (wheel[i].updateContact(dt, wheel_contact[wcount]))
		{
			wheel_contact[wcount].id = i;

			MotorJoint & joint = motor_joint[mcount];
			joint.shaft = &wheel[i].shaft;
			joint.impulseLimit = 0;
			joint.targetVelocity = wheel_contact[wcount].v1 / wheel[i].getRadius();
			joint.accumulatedImpulse = 0;
			abs_active |= wheel[i].getABS();
			tcs_active |= wheel[i].getTCS();
			mcount++;
			wcount++;
		}
	}

	// engine
	MotorJoint & joint = motor_joint[mcount];
	joint.shaft = &engine.getShaft();
	joint.targetVelocity = engine.getTorque() > 0 ? engine.getRPMLimit() * M_PI / 30 : 0;
	joint.impulseLimit = btFabs(engine.getTorque()) * dt;
	joint.accumulatedImpulse = 0;
	mcount++;

	// brakes
	for (int i = 0; i < wheel.size(); ++i)
	{
		btScalar torque = wheel[i].brake.getTorque();
		if (torque > 0)
		{
			MotorJoint & joint = motor_joint[mcount];
			joint.shaft = &wheel[i].shaft;
			joint.targetVelocity = 0;
			joint.impulseLimit = torque * dt;
			joint.accumulatedImpulse = 0;
			mcount++;
		}
	}

	// steering feedback
	steering_torque = 0;

	// solver loop
	const int iterations = 8;
	for (int n = 0; n < iterations; ++n)
	{
		// wheel
		for (int i = 0; i < wcount; ++i)
		{
			WheelContact & c = wheel_contact[i];
			Wheel & w = wheel[c.id];

			SolveConstraintRow(c.response, *c.bodyA, *c.bodyB, c.rA, c.rB);

			btScalar load = c.response.accumImpulse / dt;
			btVector3 friction = w.tire.getForce(load, c.frictionCoeff, c.camber, c.vR, c.v1, c.v2);

			// lateral friction constraint
			if (friction[1] * dt > c.friction2.upperLimit)
			{
				c.friction2.upperLimit = friction[1] * dt;
			}
			else if (friction[1] * dt < c.friction2.lowerLimit)
			{
				c.friction2.lowerLimit = friction[1] * dt;
			}
			//clog << friction[0] << " " << w.tire.getSlide() << "; ";

			// tire friction torque
			btScalar impulseLimit = btFabs(friction[0]) * w.getRadius() * dt;
			if (impulseLimit > motor_joint[i].impulseLimit)
			{
				motor_joint[i].impulseLimit = impulseLimit;
			}
			motor_joint[i].targetVelocity = c.v1 / w.getRadius();
		}

		// driveline
		for (int i = 0; i < mcount; ++i)
		{
			motor_joint[i].solve();
		}
		for (int i = 0; i < dcount; ++i)
		{
			diff_joint[i].solve();
		}
		for (int i = 0; i < ccount; ++i)
		{
			clutch_joint[i].solve();
		}
		//clog << "  ";

		// wheel friction
		for (int i = 0; i < wcount; ++i)
		{
			WheelContact & c = wheel_contact[i];
			Wheel & w = wheel[c.id];

			// longitudinal friction costraint from tire friction torque
			btScalar impulseLimit = -motor_joint[i].accumulatedImpulse / w.getRadius();
			if (impulseLimit > c.friction1.upperLimit)
			{
				c.friction1.upperLimit = impulseLimit;
			}
			else if (impulseLimit < c.friction1.lowerLimit)
			{
				c.friction1.lowerLimit = impulseLimit;
			}
			//clog << -impulseLimit / w.getRadius() << " ";

			btScalar vel = w.shaft.getAngularVelocity() * w.getRadius();
			SolveConstraintRow(c.friction1, *c.bodyA, *c.bodyB, c.rA, c.rB, -vel);
			SolveConstraintRow(c.friction2, *c.bodyA, *c.bodyB, c.rA, c.rB);

			// steering feedback
			if (w.suspension.getMaxSteeringAngle() > 1E-3)
			{
				steering_torque += w.tire.getMz();
			}
		}
		//clog << "\n";
	}
	//clog << "\n";
	steering_torque /= iterations;
}
void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
																	btMultiBodyJacobianData& data,
																 const btVector3& contactNormalOnB,
																 const btVector3& posAworld, const btVector3& posBworld, 
																 btScalar position,
																 const btContactSolverInfo& infoGlobal,
																 btScalar& relaxation,
																 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
{
			
	
	btVector3 rel_pos1 = posAworld;
	btVector3 rel_pos2 = posBworld;

	solverConstraint.m_multiBodyA = m_bodyA;
	solverConstraint.m_multiBodyB = m_bodyB;
	solverConstraint.m_linkA = m_linkA;
	solverConstraint.m_linkB = m_linkB;
	

	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;

	const btVector3& pos1 = posAworld;
	const btVector3& pos2 = posBworld;

	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);

	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;

	if (bodyA)
		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
	if (bodyB)
		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();

	relaxation = 1.f;

	if (multiBodyA)
	{
		const int ndofA  = multiBodyA->getNumLinks() + 6;

		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();

		if (solverConstraint.m_deltaVelAindex <0)
		{
			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
		} else
		{
			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
		}

		solverConstraint.m_jacAindex = data.m_jacobians.size();
		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());

		btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
		multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
	} else
	{
		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
		solverConstraint.m_contactNormal1 = contactNormalOnB;
	}

	if (multiBodyB)
	{
		const int ndofB  = multiBodyB->getNumLinks() + 6;

		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
		if (solverConstraint.m_deltaVelBindex <0)
		{
			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
		}

		solverConstraint.m_jacBindex = data.m_jacobians.size();

		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());

		multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
	} else
	{
		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
		solverConstraint.m_contactNormal2 = -contactNormalOnB;
	}

	{
						
		btVector3 vec;
		btScalar denom0 = 0.f;
		btScalar denom1 = 0.f;
		btScalar* jacB = 0;
		btScalar* jacA = 0;
		btScalar* lambdaA =0;
		btScalar* lambdaB =0;
		int ndofA  = 0;
		if (multiBodyA)
		{
			ndofA  = multiBodyA->getNumLinks() + 6;
			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
			lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
			for (int i = 0; i < ndofA; ++i)
			{
				btScalar j = jacA[i] ;
				btScalar l =lambdaA[i];
				denom0 += j*l;
			}
		} else
		{
			if (rb0)
			{
				vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
				denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
			}
		}
		if (multiBodyB)
		{
			const int ndofB  = multiBodyB->getNumLinks() + 6;
			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
			lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
			for (int i = 0; i < ndofB; ++i)
			{
				btScalar j = jacB[i] ;
				btScalar l =lambdaB[i];
				denom1 += j*l;
			}

		} else
		{
			if (rb1)
			{
				vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
				denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
			}
		}

		 if (multiBodyA && (multiBodyA==multiBodyB))
		 {
            // ndof1 == ndof2 in this case
            for (int i = 0; i < ndofA; ++i) 
			{
                denom1 += jacB[i] * lambdaA[i];
                denom1 += jacA[i] * lambdaB[i];
            }
        }

		 btScalar d = denom0+denom1;
		 if (btFabs(d)>SIMD_EPSILON)
		 {
			 
			 solverConstraint.m_jacDiagABInv = relaxation/(d);
		 } else
		 {
			solverConstraint.m_jacDiagABInv  = 1.f;
		 }
		
	}

	
	//compute rhs and remaining solverConstraint fields

	

	btScalar restitution = 0.f;
	btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;

	btScalar rel_vel = 0.f;
	int ndofA  = 0;
	int ndofB  = 0;
	{

		btVector3 vel1,vel2;
		if (multiBodyA)
		{
			ndofA  = multiBodyA->getNumLinks() + 6;
			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
			for (int i = 0; i < ndofA ; ++i) 
				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
		} else
		{
			if (rb0)
			{
				rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
			}
		}
		if (multiBodyB)
		{
			ndofB  = multiBodyB->getNumLinks() + 6;
			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
			for (int i = 0; i < ndofB ; ++i) 
				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];

		} else
		{
			if (rb1)
			{
				rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
			}
		}

		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;

				
		restitution =  restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
		if (restitution <= btScalar(0.))
		{
			restitution = 0.f;
		};
	}


	///warm starting (or zero if disabled)
	/*
	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
	{
		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;

		if (solverConstraint.m_appliedImpulse)
		{
			if (multiBodyA)
			{
				btScalar impulse = solverConstraint.m_appliedImpulse;
				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
				multiBodyA->applyDeltaVee(deltaV,impulse);
				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
			} else
			{
				if (rb0)
					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
			}
			if (multiBodyB)
			{
				btScalar impulse = solverConstraint.m_appliedImpulse;
				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
				multiBodyB->applyDeltaVee(deltaV,impulse);
				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
			} else
			{
				if (rb1)
					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
			}
		}
	} else
	*/
	{
		solverConstraint.m_appliedImpulse = 0.f;
	}

	solverConstraint.m_appliedPushImpulse = 0.f;

	{
		

		btScalar positionalError = 0.f;
		btScalar	velocityError = restitution - rel_vel;// * damping;
					

		btScalar erp = infoGlobal.m_erp2;
		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
		{
			erp = infoGlobal.m_erp;
		}

		if (penetration>0)
		{
			positionalError = 0;
			velocityError = -penetration / infoGlobal.m_timeStep;

		} else
		{
			positionalError = -penetration * erp/infoGlobal.m_timeStep;
		}

		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;

		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
		{
			//combine position and velocity into rhs
			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
			solverConstraint.m_rhsPenetration = 0.f;

		} else
		{
			//split position and velocity into rhs and m_rhsPenetration
			solverConstraint.m_rhs = velocityImpulse;
			solverConstraint.m_rhsPenetration = penetrationImpulse;
		}

		solverConstraint.m_cfm = 0.f;
		solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
		solverConstraint.m_upperLimit = m_maxAppliedImpulse;
	}

}
Example #28
0
void btContactArray::merge_contacts(
	const btContactArray & contacts, bool normal_contact_average)
{
	clear();

	int i;
	if(contacts.size()==0) return;


	if(contacts.size()==1)
	{
		push_back(contacts[0]);
		return;
	}

	btAlignedObjectArray<CONTACT_KEY_TOKEN> keycontacts;

	keycontacts.reserve(contacts.size());

	//fill key contacts

	for ( i = 0;i<contacts.size() ;i++ )
	{
		keycontacts.push_back(CONTACT_KEY_TOKEN(contacts[i].calc_key_contact(),i));
	}

	//sort keys
	keycontacts.quickSort(CONTACT_KEY_TOKEN_COMP());

	// Merge contacts
	int coincident_count=0;
	btVector3 coincident_normals[MAX_COINCIDENT];

	unsigned int last_key = keycontacts[0].m_key;
	unsigned int key = 0;

	push_back(contacts[keycontacts[0].m_value]);

	GIM_CONTACT * pcontact = &(*this)[0];

	for( i=1;i<keycontacts.size();i++)
	{
	    key = keycontacts[i].m_key;
		const GIM_CONTACT * scontact = &contacts[keycontacts[i].m_value];

		if(last_key ==  key)//same points
		{
			//merge contact
			if(pcontact->m_depth - CONTACT_DIFF_EPSILON > scontact->m_depth)//)
			{
				*pcontact = *scontact;
                coincident_count = 0;
			}
			else if(normal_contact_average)
			{
				if(btFabs(pcontact->m_depth - scontact->m_depth)<CONTACT_DIFF_EPSILON)
                {
                    if(coincident_count<MAX_COINCIDENT)
                    {
                    	coincident_normals[coincident_count] = scontact->m_normal;
                        coincident_count++;
                    }
                }
			}
		}
		else
		{//add new contact

		    if(normal_contact_average && coincident_count>0)
		    {
		    	pcontact->interpolate_normals(coincident_normals,coincident_count);
		        coincident_count = 0;
		    }

		    push_back(*scontact);
		    pcontact = &(*this)[this->size()-1];
        }
		last_key = key;
	}
}
void	btConvexPolyhedron::initialize()
{

	btHashMap<btInternalVertexPair,btInternalEdge> edges;

	btScalar TotalArea = 0.0f;
	
	m_localCenter.setValue(0, 0, 0);
	for(int i=0;i<m_faces.size();i++)
	{
		int numVertices = m_faces[i].m_indices.size();
		int NbTris = numVertices;
		for(int j=0;j<NbTris;j++)
		{
			int k = (j+1)%numVertices;
			btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
			btInternalEdge* edptr = edges.find(vp);
			btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0];
			edge.normalize();

			bool found = false;

			for (int p=0;p<m_uniqueEdges.size();p++)
			{
				
				if (IsAlmostZero(m_uniqueEdges[p]-edge) || 
					IsAlmostZero(m_uniqueEdges[p]+edge))
				{
					found = true;
					break;
				}
			}

			if (!found)
			{
				m_uniqueEdges.push_back(edge);
			}

			if (edptr)
			{
#if 0
				btAssert(edptr->m_face0>=0);
				btAssert(edptr->m_face1<0);
#else
				// why I am getting this asserts on some maps?
#endif
				edptr->m_face1 = i;
			} else
			{
				btInternalEdge ed;
				ed.m_face0 = i;
				edges.insert(vp,ed);
			}
		}
	}

#ifdef USE_CONNECTED_FACES
	for(int i=0;i<m_faces.size();i++)
	{
		int numVertices = m_faces[i].m_indices.size();
		m_faces[i].m_connectedFaces.resize(numVertices);

		for(int j=0;j<numVertices;j++)
		{
			int k = (j+1)%numVertices;
			btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]);
			btInternalEdge* edptr = edges.find(vp);
			btAssert(edptr);
			btAssert(edptr->m_face0>=0);
			btAssert(edptr->m_face1>=0);

			int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0;
			m_faces[i].m_connectedFaces[j] = connectedFace;
		}
	}
#endif//USE_CONNECTED_FACES

	for(int i=0;i<m_faces.size();i++)
	{
		int numVertices = m_faces[i].m_indices.size();
		int NbTris = numVertices-2;
		
		const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]];
		for(int j=1;j<=NbTris;j++)
		{
			int k = (j+1)%numVertices;
			const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]];
			const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]];
			btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
			btVector3 Center = (p0+p1+p2)/3.0f;
			m_localCenter += Area * Center;
			TotalArea += Area;
		}
	}
	m_localCenter /= TotalArea;




#ifdef TEST_INTERNAL_OBJECTS
	if(1)
	{
		m_radius = FLT_MAX;
		for(int i=0;i<m_faces.size();i++)
		{
			const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
			const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
			if(dist<m_radius)
				m_radius = dist;
		}

	
		btScalar MinX = FLT_MAX;
		btScalar MinY = FLT_MAX;
		btScalar MinZ = FLT_MAX;
		btScalar MaxX = -FLT_MAX;
		btScalar MaxY = -FLT_MAX;
		btScalar MaxZ = -FLT_MAX;
		for(int i=0; i<m_vertices.size(); i++)
		{
			const btVector3& pt = m_vertices[i];
			if(pt.x()<MinX)	MinX = pt.x();
			if(pt.x()>MaxX)	MaxX = pt.x();
			if(pt.y()<MinY)	MinY = pt.y();
			if(pt.y()>MaxY)	MaxY = pt.y();
			if(pt.z()<MinZ)	MinZ = pt.z();
			if(pt.z()>MaxZ)	MaxZ = pt.z();
		}
		mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ);
		mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ);



//		const btScalar r = m_radius / sqrtf(2.0f);
		const btScalar r = m_radius / sqrtf(3.0f);
		const int LargestExtent = mE.maxAxis();
		const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f;
		m_extents[0] = m_extents[1] = m_extents[2] = r;
		m_extents[LargestExtent] = mE[LargestExtent]*0.5f;
		bool FoundBox = false;
		for(int j=0;j<1024;j++)
		{
			if(testContainment())
			{
				FoundBox = true;
				break;
			}

			m_extents[LargestExtent] -= Step;
		}
		if(!FoundBox)
		{
			m_extents[0] = m_extents[1] = m_extents[2] = r;
		}
		else
		{
			// Refine the box
			const btScalar Step = (m_radius - r)/1024.0f;
			const int e0 = (1<<LargestExtent) & 3;
			const int e1 = (1<<e0) & 3;

			for(int j=0;j<1024;j++)
			{
				const btScalar Saved0 = m_extents[e0];
				const btScalar Saved1 = m_extents[e1];
				m_extents[e0] += Step;
				m_extents[e1] += Step;

				if(!testContainment())
				{
					m_extents[e0] = Saved0;
					m_extents[e1] = Saved1;
					break;
				}
			}
		}
	}
#endif
}
void	btConeTwistConstraint::buildJacobian()
{
	m_appliedImpulse = btScalar(0.);

	//set bias, sign, clear accumulator
	m_swingCorrection = btScalar(0.);
	m_twistLimitSign = btScalar(0.);
	m_solveTwistLimit = false;
	m_solveSwingLimit = false;
	m_accTwistLimitImpulse = btScalar(0.);
	m_accSwingLimitImpulse = btScalar(0.);

	if (!m_angularOnly)
	{
		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
		btVector3 relPos = pivotBInW - pivotAInW;

		btVector3 normal[3];
		if (relPos.length2() > SIMD_EPSILON)
		{
			normal[0] = relPos.normalized();
		}
		else
		{
			normal[0].setValue(btScalar(1.0),0,0);
		}

		btPlaneSpace1(normal[0], normal[1], normal[2]);

		for (int i=0;i<3;i++)
		{
			new (&m_jac[i]) btJacobianEntry(
				m_rbA.getCenterOfMassTransform().getBasis().transpose(),
				m_rbB.getCenterOfMassTransform().getBasis().transpose(),
				pivotAInW - m_rbA.getCenterOfMassPosition(),
				pivotBInW - m_rbB.getCenterOfMassPosition(),
				normal[i],
				m_rbA.getInvInertiaDiagLocal(),
				m_rbA.getInvMass(),
				m_rbB.getInvInertiaDiagLocal(),
				m_rbB.getInvMass());
		}
	}

	btVector3 b1Axis1,b1Axis2,b1Axis3;
	btVector3 b2Axis1,b2Axis2;

	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);

	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);

	// Get Frame into world space
	if (m_swingSpan1 >= btScalar(0.05f))
	{
		b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
		swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
	}

	if (m_swingSpan2 >= btScalar(0.05f))
	{
		b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);			
		swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
	}

	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);		
	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);	
	btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq;

	if (EllipseAngle > 1.0f)
	{
		m_swingCorrection = EllipseAngle-1.0f;
		m_solveSwingLimit = true;
		
		// Calculate necessary axis & factors
		m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
		m_swingAxis.normalize();

		btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
		m_swingAxis *= swingAxisSign;

		m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
			getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));

	}

	// Twist limits
	if (m_twistSpan >= btScalar(0.))
	{
		btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
		btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
		btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
		btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );

		btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
		if (twist <= -m_twistSpan*lockedFreeFactor)
		{
			m_twistCorrection = -(twist + m_twistSpan);
			m_solveTwistLimit = true;

			m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
			m_twistAxis.normalize();
			m_twistAxis *= -1.0f;

			m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
				getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));

		}	else
			if (twist >  m_twistSpan*lockedFreeFactor)
			{
				m_twistCorrection = (twist - m_twistSpan);
				m_solveTwistLimit = true;

				m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
				m_twistAxis.normalize();

				m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
					getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));

			}
	}
}