C++ dgFloat32函数代码示例

dgInt32 dgCollisionTaperedCylinder::CalculateContacts (const dgVector& point, const dgVector& normal, dgCollisionParamProxy& proxy, dgVector* const contactsOut) const
{
	dgAssert (dgAbsf (normal % normal - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
	return CalculateContactsGeneric (point, normal, proxy, contactsOut);
}

void dgBilateralConstraint::JointAccelerations(dgJointAccelerationDecriptor* const params)
{
	dgJacobianMatrixElement* const jacobianMatrixElements = params->m_rowMatrix;
	const dgVector& bodyVeloc0 = m_body0->m_veloc;
	const dgVector& bodyOmega0 = m_body0->m_omega;
	const dgVector& bodyVeloc1 = m_body1->m_veloc;
	const dgVector& bodyOmega1 = m_body1->m_omega;

// remember the impulse branch 
//dgAssert (params->m_timeStep > dgFloat32 (0.0f));
	if (params->m_timeStep > dgFloat32 (0.0f)) {
		dgFloat32 kd = DG_VEL_DAMP * dgFloat32 (4.0f);
		dgFloat32 ks = DG_POS_DAMP * dgFloat32 (0.25f);
		dgFloat32 dt = params->m_timeStep;
		for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
			if (m_rowIsMotor[k]) {
				//params.m_coordenateAccel[k] = m_motorAcceleration[k] + params.m_externAccelaration[k];
   				jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
			} else {
				const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
				dgVector relVeloc (Jt.m_jacobianM0.m_linear.CompProduct3(bodyVeloc0) + Jt.m_jacobianM0.m_angular.CompProduct3(bodyOmega0) +
								   Jt.m_jacobianM1.m_linear.CompProduct3(bodyVeloc1) + Jt.m_jacobianM1.m_angular.CompProduct3(bodyOmega1));

				dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;

				//dgFloat32 aRel = jacobianMatrixElements[k].m_deltaAccel;
				//dgFloat32 aRel = jacobianMatrixElements[k].m_coordenateAccel;
				dgFloat32 aRel = params->m_firstPassCoefFlag ? jacobianMatrixElements[k].m_deltaAccel : jacobianMatrixElements[k].m_coordenateAccel;

				//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
				//alphaError = num / den;

				//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
				//dgFloat32 dt = desc.m_timestep;
				//dgFloat32 ks = DG_POS_DAMP;
				//dgFloat32 kd = DG_VEL_DAMP;
				//dgFloat32 ksd = dt * ks;
				//dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
				//dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
				//accelError = num / den;

				dgFloat32 ksd = dt * ks;
				dgFloat32 relPosit = jacobianMatrixElements[k].m_penetration - vRel * dt * params->m_firstPassCoefFlag;
				jacobianMatrixElements[k].m_penetration = relPosit;


				dgFloat32 num = ks * relPosit - kd * vRel - ksd * vRel;
				dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
				dgFloat32 aRelErr = num / den;

				//centripetal acceleration is stored in restitution member
				jacobianMatrixElements[k].m_coordenateAccel = aRelErr + jacobianMatrixElements[k].m_restitution + aRel;
			}
		}
	} else {
		for (dgInt32 k = 0; k < params->m_rowsCount; k ++) {
			if (m_rowIsMotor[k]) {
				jacobianMatrixElements[k].m_coordenateAccel = m_motorAcceleration[k] + jacobianMatrixElements[k].m_deltaAccel;
			} else {
				const dgJacobianPair& Jt = jacobianMatrixElements[k].m_Jt;
				dgVector relVeloc (Jt.m_jacobianM0.m_linear.CompProduct3(bodyVeloc0) +
								   Jt.m_jacobianM0.m_angular.CompProduct3(bodyOmega0) +
								   Jt.m_jacobianM1.m_linear.CompProduct3(bodyVeloc1) +
								   Jt.m_jacobianM1.m_angular.CompProduct3(bodyOmega1));

				dgFloat32 vRel = relVeloc.m_x + relVeloc.m_y + relVeloc.m_z;
				jacobianMatrixElements[k].m_coordenateAccel = jacobianMatrixElements[k].m_deltaAccel - vRel;
			}
		}
	}
}

dgWorld::dgWorld(dgMemoryAllocator* allocator):
//	dgThreadHive(),
	dgBodyMasterList(allocator),
	dgBroadPhaseCollision(allocator),
	dgBodyMaterialList(allocator),
	dgBodyCollisionList(allocator),
	dgActiveContacts(allocator), 
	dgCollidingPairCollector(),
	m_perInstanceData(allocator),
	m_threadsManager(),
	m_dynamicSolver()
{
	dgInt32 steps;
	dgFloat32 freezeAccel2;
	dgFloat32 freezeAlpha2;
	dgFloat32 freezeSpeed2;
	dgFloat32 freezeOmega2;

	// init exact arithmetic functions
	m_allocator = allocator;

//dgThreadHive::SetThreadCount(16);
//dgThreadHive::SetThreadCount(32);

//_control87 (_EM_ZERODIVIDE | _EM_INEXACT | _EM_OVERFLOW | _EM_INVALID, _MCW_EM);


	m_inUpdate = 0;
	m_bodyGroupID = 0;
//	m_activeBodiesCount = 0;
	
	m_defualtBodyGroupID = CreateBodyGroupID();
	m_islandMemorySizeInBytes = DG_INITIAL_ISLAND_SIZE;
	m_bodiesMemorySizeInBytes = DG_INITIAL_BODIES_SIZE;
	m_jointsMemorySizeInBytes = DG_INITIAL_JOINTS_SIZE;

	m_pairMemoryBufferSizeInBytes = 1024 * 64 * sizeof (void*);
	m_pairMemoryBuffer = m_allocator->MallocLow (m_pairMemoryBufferSizeInBytes);
	m_islandMemory = m_allocator->MallocLow (m_islandMemorySizeInBytes); 
	m_jointsMemory = m_allocator->MallocLow (m_jointsMemorySizeInBytes); 
	m_bodiesMemory = m_allocator->MallocLow (m_bodiesMemorySizeInBytes); 
	for (dgInt32 i = 0; i < DG_MAXIMUN_THREADS; i ++) {
		m_jacobiansMemorySizeInBytes[i] = DG_INITIAL_JACOBIAN_SIZE;
		m_jacobiansMemory[i] = m_allocator->MallocLow (m_jacobiansMemorySizeInBytes[i]);  

		m_internalForcesMemorySizeInBytes[i] = DG_INITIAL_BODIES_SIZE;
		m_internalForcesMemory[i] = m_allocator->MallocLow (m_internalForcesMemorySizeInBytes[i]);  

		m_contactBuffersSizeInBytes[i] = DG_INITIAL_CONTATCT_SIZE;
		m_contactBuffers[i] = m_allocator->MallocLow (m_contactBuffersSizeInBytes[i]);  
	}

	m_genericLRUMark = 0;
	m_singleIslandMultithreading = 1;

	m_solverMode = 0;
	m_frictionMode = 0;
	m_dynamicsLru = 0;
	m_broadPhaseLru = 0;
		
	m_bodiesUniqueID = 0;
//	m_bodiesCount = 0;
	m_frictiomTheshold = dgFloat32 (0.25f);

	m_userData = NULL;
	m_islandUpdate = NULL;
	m_destroyCollision = NULL;
	m_leavingWorldNotify = NULL;
	m_destroyBodyByExeciveForce = NULL;

	m_freezeAccel2 = DG_FREEZE_MAG2;
	m_freezeAlpha2 = DG_FREEZE_MAG2;
	m_freezeSpeed2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);
	m_freezeOmega2 = DG_FREEZE_MAG2 * dgFloat32 (0.1f);

	steps = 1;
	freezeAccel2 = m_freezeAccel2;
	freezeAlpha2 = m_freezeAlpha2;
	freezeSpeed2 = m_freezeSpeed2;
	freezeOmega2 = m_freezeOmega2;
	for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
		m_sleepTable[i].m_maxAccel = freezeAccel2;
		m_sleepTable[i].m_maxAlpha = freezeAlpha2;
		m_sleepTable[i].m_maxVeloc = freezeSpeed2;
		m_sleepTable[i].m_maxOmega = freezeOmega2;
		m_sleepTable[i].m_steps = steps;
		steps += 7;
		freezeAccel2 *= dgFloat32 (1.5f);
		freezeAlpha2 *= dgFloat32 (1.4f);
		freezeSpeed2 *= dgFloat32 (1.5f);
		freezeOmega2 *= dgFloat32 (1.5f);
	}

	steps += 300;
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAccel *= dgFloat32 (100.0f);
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxAlpha *= dgFloat32 (100.0f);
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxVeloc = 0.25f;
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_maxOmega = 0.1f;
	m_sleepTable[DG_SLEEP_ENTRIES - 1].m_steps = steps;

//.........这里部分代码省略.........

dgVector dgCollisionSphere::SupportVertexSpecial (const dgVector& dir, dgFloat32 skinThickness, dgInt32* const vertexIndex) const 
{
	return dgVector (dgFloat32 (0.0f));
}

void dgBilateralConstraint::CalculateAngularDerivative (dgInt32 index, dgContraintDescritor& desc, const dgVector& dir,	dgFloat32 stiffness, dgFloat32 jointAngle, dgForceImpactPair* const jointForce)
{
	dgAssert (jointForce);
	dgAssert (m_body0);
	dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0; 
	jacobian0.m_linear[0] = dgFloat32 (0.0f);
	jacobian0.m_linear[1] = dgFloat32 (0.0f);
	jacobian0.m_linear[2] = dgFloat32 (0.0f);
	jacobian0.m_linear[3] = dgFloat32 (0.0f);
	jacobian0.m_angular[0] = dir.m_x;
	jacobian0.m_angular[1] = dir.m_y;
	jacobian0.m_angular[2] = dir.m_z;
	jacobian0.m_angular[3] = dgFloat32 (0.0f);

	dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1; 
	dgAssert (m_body1);
	jacobian1.m_linear[0] = dgFloat32 (0.0f);
	jacobian1.m_linear[1] = dgFloat32 (0.0f);
	jacobian1.m_linear[2] = dgFloat32 (0.0f);
	jacobian1.m_linear[3] = dgFloat32 (0.0f);
	jacobian1.m_angular[0] = -dir.m_x;
	jacobian1.m_angular[1] = -dir.m_y;
	jacobian1.m_angular[2] = -dir.m_z;
	jacobian1.m_angular[3] = dgFloat32 (0.0f);

	const dgVector& omega0 = m_body0->GetOmega();
	const dgVector& omega1 = m_body1->GetOmega();
	dgFloat32 omegaError = (omega1 - omega0) % dir;

	m_rowIsMotor[index] = 0;
	m_motorAcceleration[index] = dgFloat32 (0.0f);

	if (desc.m_timestep > dgFloat32 (0.0f)) {

		//at =  [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks] 
		dgFloat32 dt = desc.m_timestep;
		dgFloat32 ks = DG_POS_DAMP;
		dgFloat32 kd = DG_VEL_DAMP;
		dgFloat32 ksd = dt * ks;
		dgFloat32 num = ks * jointAngle + kd * omegaError + ksd * omegaError;
		dgFloat32 den = dgFloat32 (1.0f) + dt * kd + dt * ksd;
		dgFloat32 alphaError = num / den;

		desc.m_zeroRowAcceleration[index] = (jointAngle + omegaError * desc.m_timestep) * desc.m_invTimestep * desc.m_invTimestep;

		desc.m_penetration[index] = jointAngle;
		desc.m_jointAccel[index] = alphaError;
		desc.m_restitution[index] = dgFloat32 (0.0f);
		desc.m_jointStiffness[index] = stiffness;
		desc.m_penetrationStiffness[index] = dgFloat32 (0.0f);
		desc.m_forceBounds[index].m_jointForce = jointForce;
	} else {
		desc.m_penetration[index] = dgFloat32 (0.0f);
		desc.m_jointAccel[index] = omegaError;
		desc.m_restitution[index] = dgFloat32 (0.0f);
		desc.m_jointStiffness[index] = stiffness;
		desc.m_penetrationStiffness[index] = dgFloat32 (0.0f);
		desc.m_zeroRowAcceleration[index]  = dgFloat32 (0.0f);
		desc.m_forceBounds[index].m_jointForce = jointForce;
	}
}

void dgCollisionSphere::TesselateTriangle (dgInt32 level, const dgVector& p0, const dgVector& p1, const dgVector& p2, dgInt32& count, dgVector* const ouput) const
{
	if (level) {
		dgAssert (dgAbs (p0.DotProduct(p0).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
		dgAssert (dgAbs (p1.DotProduct(p1).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
		dgAssert (dgAbs (p2.DotProduct(p2).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
		dgVector p01 (p0 + p1);
		dgVector p12 (p1 + p2);
		dgVector p20 (p2 + p0);

		//p01 = p01 * p01.InvMagSqrt();
		//p12 = p12 * p12.InvMagSqrt();
		//p20 = p20 * p20.InvMagSqrt();

		p01 = p01.Normalize();
		p12 = p12.Normalize();
		p20 = p20.Normalize();

		dgAssert (dgAbs (p01.DotProduct(p01).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
		dgAssert (dgAbs (p12.DotProduct(p12).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));
		dgAssert (dgAbs (p20.DotProduct(p20).GetScalar() - dgFloat32 (1.0f)) < dgFloat32 (1.0e-4f));

		TesselateTriangle (level - 1, p0,  p01, p20, count, ouput);
		TesselateTriangle (level - 1, p1,  p12, p01, count, ouput);
		TesselateTriangle (level - 1, p2,  p20, p12, count, ouput);
		TesselateTriangle (level - 1, p01, p12, p20, count, ouput);

	} else {
		ouput[count ++] = p0;
		ouput[count ++] = p1;
		ouput[count ++] = p2;
	}
}

dgInt32 dgCollisionBox::CalculatePlaneIntersection (const dgVector& normal, const dgVector& point, dgVector* const contactsOut) const
{
	dgVector support[4];
	dgInt32 featureCount = 3;
	
	const dgConvexSimplexEdge** const vertToEdgeMapping = GetVertexToEdgeMapping();
	if (vertToEdgeMapping) {
		dgInt32 edgeIndex;
		//support[0] = SupportVertex (normal.Scale4(normalSign), &edgeIndex);
		support[0] = SupportVertex (normal, &edgeIndex);

		dgFloat32 dist = normal.DotProduct4(support[0] - point).GetScalar();
		if (dist <= DG_IMPULSIVE_CONTACT_PENETRATION) {
			dgVector normalAlgin (normal.Abs());
			if (!((normalAlgin.m_x > dgFloat32 (0.9999f)) || (normalAlgin.m_y > dgFloat32 (0.9999f)) || (normalAlgin.m_z > dgFloat32 (0.9999f)))) {
				// 0.25 degrees
				const dgFloat32 tiltAngle = dgFloat32 (0.005f);
				const dgFloat32 tiltAngle2 = tiltAngle * tiltAngle ;
				dgPlane testPlane (normal, - (normal.DotProduct4(support[0]).GetScalar()));

				featureCount = 1;
				const dgConvexSimplexEdge* const edge = vertToEdgeMapping[edgeIndex];
				const dgConvexSimplexEdge* ptr = edge;
				do {
					const dgVector& p = m_vertex[ptr->m_twin->m_vertex];
					dgFloat32 test1 = testPlane.Evalue(p);
					dgVector dist (p - support[0]);
					dgFloat32 angle2 = test1 * test1 / (dist.DotProduct4(dist).GetScalar());
					if (angle2 < tiltAngle2) {
						support[featureCount] = p;
						featureCount ++;
					}
					ptr = ptr->m_twin->m_next;
				} while ((ptr != edge) && (featureCount < 3));
			}
		}
	}

	dgInt32 count = 0;
	switch (featureCount)
	{
		case 1:
		{
			contactsOut[0] = support[0] - normal.CompProduct4(normal.DotProduct4(support[0] - point));
			count = 1;
			break;
		}

		case 2:
		{
			contactsOut[0] = support[0] - normal.CompProduct4(normal.DotProduct4(support[0] - point));
			contactsOut[1] = support[1] - normal.CompProduct4(normal.DotProduct4(support[1] - point));
			count = 2;
			break;
		}

		default:
		{
			dgFloat32 test[8];
			dgAssert(normal.m_w == dgFloat32(0.0f));
			dgPlane plane(normal, -(normal.DotProduct4(point).GetScalar()));
			for (dgInt32 i = 0; i < 8; i++) {
				dgAssert(m_vertex[i].m_w == dgFloat32(0.0f));
				test[i] = plane.DotProduct4(m_vertex[i] | dgVector::m_wOne).m_x;
			}

			dgConvexSimplexEdge* edge = NULL;
			for (dgInt32 i = 0; i < dgInt32 (sizeof (m_edgeEdgeMap) / sizeof (m_edgeEdgeMap[0])); i ++) {
				dgConvexSimplexEdge* const ptr = m_edgeEdgeMap[i];
				dgFloat32 side0 = test[ptr->m_vertex];
				dgFloat32 side1 = test[ptr->m_twin->m_vertex];
				if ((side0 * side1) < dgFloat32 (0.0f)) {
					edge = ptr;
					break;
				}
			}

			if (edge) {
				if (test[edge->m_vertex] < dgFloat32 (0.0f)) {
					edge = edge->m_twin;
				}
				dgAssert (test[edge->m_vertex] > dgFloat32 (0.0f));

				dgConvexSimplexEdge* ptr = edge;
				dgConvexSimplexEdge* firstEdge = NULL;
				dgFloat32 side0 = test[edge->m_vertex];
				do {
					dgAssert (m_vertex[ptr->m_twin->m_vertex].m_w == dgFloat32 (0.0f));
					dgFloat32 side1 = test[ptr->m_twin->m_vertex];
					if (side1 < side0) {
						if (side1 < dgFloat32 (0.0f)) {
							firstEdge = ptr;
							break;
						}

						side0 = side1;
						edge = ptr->m_twin;
						ptr = edge;
					}
					ptr = ptr->m_twin->m_next;
//.........这里部分代码省略.........

void dgCollisionBox::Init (dgFloat32 size_x, dgFloat32 size_y, dgFloat32 size_z)
{
	m_rtti |= dgCollisionBox_RTTI;
	m_size[0].m_x = dgMax (dgAbsf (size_x) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
	m_size[0].m_y = dgMax (dgAbsf (size_y) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
	m_size[0].m_z = dgMax (dgAbsf (size_z) * dgFloat32 (0.5f), dgFloat32(2.0f) * D_BOX_SKIN_THINCKNESS);
	m_size[0].m_w = dgFloat32 (0.0f);

	m_size[1].m_x = - m_size[0].m_x;
	m_size[1].m_y = - m_size[0].m_y;
	m_size[1].m_z = - m_size[0].m_z;
	m_size[1].m_w = dgFloat32 (0.0f);

	m_edgeCount = 24;
	m_vertexCount = 8;

	m_vertex[0]	= dgVector ( m_size[0].m_x,  m_size[0].m_y,  m_size[0].m_z, dgFloat32 (0.0f));
	m_vertex[1]	= dgVector (-m_size[0].m_x,  m_size[0].m_y,  m_size[0].m_z, dgFloat32 (0.0f));
	m_vertex[2]	= dgVector ( m_size[0].m_x, -m_size[0].m_y,  m_size[0].m_z, dgFloat32 (0.0f));
	m_vertex[3]	= dgVector (-m_size[0].m_x, -m_size[0].m_y,  m_size[0].m_z, dgFloat32 (0.0f));

	m_vertex[4]	= dgVector ( m_size[0].m_x,  m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
	m_vertex[5]	= dgVector (-m_size[0].m_x,  m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
	m_vertex[6]	= dgVector ( m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));
	m_vertex[7]	= dgVector (-m_size[0].m_x, -m_size[0].m_y, -m_size[0].m_z, dgFloat32 (0.0f));

	dgCollisionConvex::m_vertex = m_vertex;
	dgCollisionConvex::m_simplex = m_edgeArray;

	if (!m_initSimplex) {
		dgPolyhedra polyhedra (GetAllocator());
		polyhedra.BeginFace();
		for (dgInt32 i = 0; i < 6; i ++) {
			polyhedra.AddFace (4, &m_faces[i][0]);
		}
		polyhedra.EndFace();

		int index = 0;
		dgInt32 mark = polyhedra.IncLRU();;
		dgPolyhedra::Iterator iter (polyhedra);
		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &iter.GetNode()->GetInfo();
			if (edge->m_mark != mark) {
				dgEdge* ptr = edge;
				do {
					ptr->m_mark = mark;
					ptr->m_userData = index;
					index ++;
					ptr = ptr->m_twin->m_next;
				} while (ptr != edge) ;
			}
		}
		dgAssert (index == 24);

		polyhedra.IncLRU();
		mark = polyhedra.IncLRU();
		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &iter.GetNode()->GetInfo();
			dgEdge *ptr = edge;
			do {
				ptr->m_mark = mark;
				dgConvexSimplexEdge* const simplexPtr = &m_simplex[ptr->m_userData];
				simplexPtr->m_vertex = ptr->m_incidentVertex;
				simplexPtr->m_next = &m_simplex[ptr->m_next->m_userData];
				simplexPtr->m_prev = &m_simplex[ptr->m_prev->m_userData];
				simplexPtr->m_twin = &m_simplex[ptr->m_twin->m_userData];
				ptr = ptr->m_twin->m_next;
			} while (ptr != edge) ;
		} 

		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &iter.GetNode()->GetInfo();
			m_vertexToEdgeMap[edge->m_incidentVertex] = &m_simplex[edge->m_userData];
		}

		dgInt32 count = 0;
		mark = polyhedra.IncLRU();
		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &iter.GetNode()->GetInfo();
			if (edge->m_mark != mark) {
				edge->m_mark = mark;
				edge->m_twin->m_mark = mark;
				m_edgeEdgeMap[count] = &m_simplex[edge->m_userData];
				count ++;
				dgAssert (count <= 12);
			}
		}

		m_initSimplex = 1;
	}

	SetVolumeAndCG ();
}

dgFloat32 dgCollisionBox::RayCast (const dgVector& localP0, const dgVector& localP1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
	dgAssert (localP0.m_w == dgFloat32 (0.0f));
	dgAssert (localP1.m_w == dgFloat32 (0.0f));

	dgInt32 index = 0;
	dgFloat32 signDir = dgFloat32 (0.0f);
	dgFloat32 tmin = dgFloat32 (0.0f);
	dgFloat32 tmax = dgFloat32 (1.0f);
	for (dgInt32 i = 0; i < 3; i++) {
		dgFloat32 dp = localP1[i] - localP0[i];
		if (dgAbsf (dp) < dgFloat32 (1.0e-8f)) {
			if (localP0[i] <= m_size[1][i] || localP0[i] >= m_size[0][i]) {
				return dgFloat32 (1.2f);
			}
		} else {
			dp = dgFloat32 (1.0f) / dp; 
			dgFloat32 t1 = (m_size[1][i] - localP0[i]) * dp;
			dgFloat32 t2 = (m_size[0][i] - localP0[i]) * dp;

			dgFloat32 sign = dgFloat32 (-1.0f);
			if (t1 > t2) {
				sign = 1;
				dgSwap(t1, t2);
			}
			if (t1 > tmin) {
				signDir = sign;
				index = i;
				tmin = t1;
			}
			if (t2 < tmax) {
				tmax = t2;
			}
			if (tmin > tmax) {
				return dgFloat32 (1.2f);
			}
		}
	}

	if (tmin > dgFloat32 (0.0f)) {
		dgAssert (tmin <= 1.0f);
		contactOut.m_normal = dgVector (dgFloat32 (0.0f));
		contactOut.m_normal[index] = signDir;
		//contactOut.m_userId = SetUserDataID();
	} else {
		tmin = dgFloat32 (1.2f);
	}
	return tmin;

}

void dgCollisionBox::Serialize(dgSerialize callback, void* const userData) const
{
	SerializeLow(callback, userData);
	dgVector size (m_size[0].Scale3 (dgFloat32 (2.0f)));
	callback (userData, &size, sizeof (dgVector));
}

void dgCollisionTaperedCylinder::Init (dgFloat32 radio0, dgFloat32 radio1, dgFloat32 height)
{
	m_rtti |= dgCollisionTaperedCylinder_RTTI;
	m_radio0 = dgAbsf (radio0);
	m_radio1 = dgAbsf (radio1);
	m_height = dgAbsf (height * dgFloat32 (0.5f));

	m_dirVector.m_x = radio1 - radio0;
	m_dirVector.m_y = m_height * dgFloat32 (2.0f);
	m_dirVector.m_z = dgFloat32 (0.0f);
	m_dirVector.m_w = dgFloat32 (0.0f);
	m_dirVector = m_dirVector.Scale3(dgRsqrt(m_dirVector % m_dirVector));

	dgFloat32 angle = dgFloat32 (0.0f);
	for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
		dgFloat32 sinAngle = dgSin (angle);
		dgFloat32 cosAngle = dgCos (angle);
		m_vertex[i                       ] = dgVector (- m_height, m_radio1 * cosAngle, m_radio1 * sinAngle, dgFloat32 (0.0f));
		m_vertex[i + DG_CYLINDER_SEGMENTS] = dgVector (  m_height, m_radio0 * cosAngle, m_radio0 * sinAngle,  dgFloat32 (0.0f));
		angle += dgPI2 / DG_CYLINDER_SEGMENTS;
	}

	m_edgeCount = DG_CYLINDER_SEGMENTS * 6;
	m_vertexCount = DG_CYLINDER_SEGMENTS * 2;
	dgCollisionConvex::m_vertex = m_vertex;

	if (!m_shapeRefCount) {
		dgPolyhedra polyhedra(m_allocator);
		dgInt32 wireframe[DG_CYLINDER_SEGMENTS];

		dgInt32 j = DG_CYLINDER_SEGMENTS - 1;
		polyhedra.BeginFace ();
		for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { 
			wireframe[0] = j;
			wireframe[1] = i;
			wireframe[2] = i + DG_CYLINDER_SEGMENTS;
			wireframe[3] = j + DG_CYLINDER_SEGMENTS;
			j = i;
			polyhedra.AddFace (4, wireframe);
		}

		for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { 
			wireframe[i] = DG_CYLINDER_SEGMENTS - 1 - i;
		}
		polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);

		for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { 
			wireframe[i] = i + DG_CYLINDER_SEGMENTS;
		}
		polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
		polyhedra.EndFace ();

		dgAssert (SanityCheck (polyhedra));

		dgUnsigned64 i = 0;
		dgPolyhedra::Iterator iter (polyhedra);
		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);
			edge->m_userData = i;
			i ++;
		}

		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);

			dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
			ptr->m_vertex = edge->m_incidentVertex;
			ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
			ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
			ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
		}
	}

	m_shapeRefCount ++;
	dgCollisionConvex::m_simplex = m_edgeArray;

	SetVolumeAndCG ();
}

void dgCollisionTaperedCylinder::Serialize(dgSerialize callback, void* const userData) const
{
	dgVector size (m_radio0, m_radio1, m_height * dgFloat32 (2.0f), dgFloat32 (0.0f));
	SerializeLow(callback, userData);
	callback (userData, &size, sizeof (dgVector));
}

dgWorld::dgWorld(dgMemoryAllocator* const allocator)
	:dgBodyMasterList(allocator)
	,dgBodyMaterialList(allocator)
	,dgBodyCollisionList(allocator)
	,dgSkeletonList(allocator)
	,dgInverseDynamicsList(allocator)
	,dgContactList(allocator) 
	,dgBilateralConstraintList(allocator)
	,dgWorldDynamicUpdate(allocator)
	,dgMutexThread("newtonMainThread", 0)
	,dgWorldThreadPool(allocator)
	,dgDeadBodies(allocator)
	,dgDeadJoints(allocator)
	,dgWorldPluginList(allocator)
	,m_broadPhase(NULL)
	,m_sentinelBody(NULL)
	,m_pointCollision(NULL)
	,m_userData(NULL)
	,m_allocator (allocator)
	,m_mutex()
	,m_postUpdateCallback(NULL)
	,m_listeners(allocator)
	,m_perInstanceData(allocator)
	,m_bodiesMemory (allocator, 64)
	,m_jointsMemory (allocator, 64)
	,m_clusterMemory (allocator, 64)
	,m_solverJacobiansMemory (allocator, 64)
	,m_solverRightHandSideMemory (allocator, 64)
	,m_solverForceAccumulatorMemory (allocator, 64)
	,m_concurrentUpdate(false)
{
	//TestAStart();
	//TestSort();

	dgMutexThread* const myThread = this;
	SetParentThread (myThread);

	// avoid small memory fragmentations on initialization
	m_bodiesMemory.Resize(1024);
	m_clusterMemory.Resize(1024);
	m_jointsMemory.Resize(1024 * 2);
	m_solverJacobiansMemory.Resize(1024 * 64);
	m_solverRightHandSideMemory.Resize(1024 * 64);
	m_solverForceAccumulatorMemory.Resize(1024 * 32);

	m_savetimestep = dgFloat32 (0.0f);
	m_allocator = allocator;
	m_clusterUpdate = NULL;

	m_onCollisionInstanceDestruction = NULL;
	m_onCollisionInstanceCopyConstrutor = NULL;

	m_serializedJointCallback = NULL;	
	m_deserializedJointCallback = NULL;	

	m_inUpdate = 0;
	m_bodyGroupID = 0;
	m_lastExecutionTime = 0;
	
	m_defualtBodyGroupID = CreateBodyGroupID();
	m_genericLRUMark = 0;
	m_delayDelateLock = 0;
	m_clusterLRU = 0;

	m_useParallelSolver = 0;

	m_solverIterations = DG_DEFAULT_SOLVER_ITERATION_COUNT;
	m_dynamicsLru = 0;
	m_numberOfSubsteps = 1;
		
	m_bodiesUniqueID = 0;
	m_frictiomTheshold = dgFloat32 (0.25f);

	m_userData = NULL;
	m_clusterUpdate = NULL;

	m_freezeAccel2 = DG_FREEZE_ACCEL2;
	m_freezeAlpha2 = DG_FREEZE_ACCEL2;
	m_freezeSpeed2 = DG_FREEZE_SPEED2;
	m_freezeOmega2 = DG_FREEZE_SPEED2;

	m_contactTolerance = DG_PRUNE_CONTACT_TOLERANCE;

	dgInt32 steps = 1;
	dgFloat32 freezeAccel2 = m_freezeAccel2;
	dgFloat32 freezeAlpha2 = m_freezeAlpha2;
	dgFloat32 freezeSpeed2 = m_freezeSpeed2;
	dgFloat32 freezeOmega2 = m_freezeOmega2;
	for (dgInt32 i = 0; i < DG_SLEEP_ENTRIES; i ++) {
		m_sleepTable[i].m_maxAccel = freezeAccel2;
		m_sleepTable[i].m_maxAlpha = freezeAlpha2;
		m_sleepTable[i].m_maxVeloc = freezeSpeed2;
		m_sleepTable[i].m_maxOmega = freezeOmega2;
		m_sleepTable[i].m_steps = steps;
		steps += 7;
		freezeAccel2 *= dgFloat32 (1.5f);
		freezeAlpha2 *= dgFloat32 (1.4f);
		freezeSpeed2 *= dgFloat32 (1.5f);
		freezeOmega2 *= dgFloat32 (1.5f);
	}
//.........这里部分代码省略.........

void dgContact::JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex)
{
    dgPointParam pointData;

    dgFloat32 impulseOrForceScale = (params.m_timestep > dgFloat32 (0.0f)) ? params.m_invTimestep : dgFloat32 (1.0f);

    InitPointParam (pointData, dgFloat32 (1.0f), contact.m_point, contact.m_point);
    CalculatePointDerivative (normalIndex, params, contact.m_normal, pointData);

    dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
    dgFloat32 restitution = contact.m_restitution;

    dgFloat32 relVelocErr = velocError % contact.m_normal;

    dgFloat32 penetration = dgClamp (contact.m_penetration - DG_RESTING_CONTACT_PENETRATION, dgFloat32(0.0f), dgFloat32(0.5f));
    dgFloat32 penetrationStiffness = MAX_PENETRATION_STIFFNESS * contact.m_softness;
    dgFloat32 penetrationVeloc = penetration * penetrationStiffness;
    dgAssert (dgAbsf (penetrationVeloc - MAX_PENETRATION_STIFFNESS * contact.m_softness * penetration) < dgFloat32 (1.0e-6f));
    if (relVelocErr > REST_RELATIVE_VELOCITY) {
        relVelocErr *= (restitution + dgFloat32 (1.0f));
    }

    params.m_restitution[normalIndex] = restitution;
    params.m_penetration[normalIndex] = penetration;
    params.m_penetrationStiffness[normalIndex] = penetrationStiffness;
    params.m_forceBounds[normalIndex].m_low = dgFloat32 (0.0f);
    params.m_forceBounds[normalIndex].m_normalIndex = DG_NORMAL_CONSTRAINT;
    params.m_forceBounds[normalIndex].m_jointForce = (dgForceImpactPair*) &contact.m_normal_Force;
    params.m_jointStiffness[normalIndex] = dgFloat32 (1.0f);
    params.m_isMotor[normalIndex] = 0;

//	params.m_jointAccel[normalIndex] = GetMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * params.m_invTimestep;
    params.m_jointAccel[normalIndex] = dgMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * impulseOrForceScale;
    if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel) {
        params.m_jointAccel[normalIndex] += contact.m_normal_Force.m_force;
    }

    // first dir friction force
    if (contact.m_flags & dgContactMaterial::m_friction0Enable) {
        dgInt32 jacobIndex = frictionIndex;
        frictionIndex += 1;
        CalculatePointDerivative (jacobIndex, params, contact.m_dir0, pointData);
        relVelocErr = velocError % contact.m_dir0;
        params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
        params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);

        params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
        params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
        params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
//		if (contact.m_override0Accel) {
        if (contact.m_flags & dgContactMaterial::m_override0Accel) {
            params.m_jointAccel[jacobIndex] = contact.m_dir0_Force.m_force;
            params.m_isMotor[jacobIndex] = 1;
        } else {
            //params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
            params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
            params.m_isMotor[jacobIndex] = 0;
        }
        if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
            params.m_forceBounds[jacobIndex].m_low = -contact.m_dynamicFriction0;
            params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction0;
        } else {
            params.m_forceBounds[jacobIndex].m_low = -contact.m_staticFriction0;
            params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction0;
        }
        params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir0_Force;
    }

//	if (contact.m_friction1Enable) {
    if (contact.m_flags & dgContactMaterial::m_friction1Enable) {
        dgInt32 jacobIndex = frictionIndex;
        frictionIndex += 1;
        CalculatePointDerivative (jacobIndex, params, contact.m_dir1, pointData);
        relVelocErr = velocError % contact.m_dir1;
        params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
        params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);

        params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
        params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
        params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
//		if (contact.m_override1Accel) {
        if (contact.m_flags & dgContactMaterial::m_override1Accel) {
            dgAssert (0);
            params.m_jointAccel[jacobIndex] = contact.m_dir1_Force.m_force;
            params.m_isMotor[jacobIndex] = 1;
        } else {
            //params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
            params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
            params.m_isMotor[jacobIndex] = 0;
        }
        if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
            params.m_forceBounds[jacobIndex].m_low = - contact.m_dynamicFriction1;
            params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction1;
        } else {
            params.m_forceBounds[jacobIndex].m_low = - contact.m_staticFriction1;
            params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction1;
        }
        params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir1_Force;
    }
//dgTrace (("p(%f %f %f)\n", params.m_jointAccel[normalIndex], params.m_jointAccel[normalIndex + 1], params.m_jointAccel[normalIndex + 2]));
//.........这里部分代码省略.........

void dgWorld::SetContactMergeTolerance(dgFloat32 tolerenace)
{
	m_contactTolerance = dgMax (tolerenace, dgFloat32 (1.e-3f));
}