void dgWorld::InitBody (dgBody* const body, dgCollisionInstance* const collision, const dgMatrix& matrix)
{
dgAssert (collision);
m_bodiesUniqueID ++;
body->m_world = this;
body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
body->m_uniqueID = dgInt32 (m_bodiesUniqueID);
dgBodyMasterList::AddBody(body);
body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f)));
body->SetLinearDamping (dgFloat32 (0.1045f)) ;
body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));
body->AttachCollision(collision);
body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);
dgMatrix inertia (dgGetIdentityMatrix());
inertia[0][0] = DG_INFINITE_MASS;
inertia[1][1] = DG_INFINITE_MASS;
inertia[2][2] = DG_INFINITE_MASS;
body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), inertia);
body->SetMatrix (matrix);
if (!body->GetCollision()->IsType (dgCollision::dgCollisionNull_RTTI)) {
m_broadPhase->Add (body);
}
}
dgFloat32 dgCollisionScene::RayCast(const dgVector& localP0,
const dgVector& localP1, dgContactPoint& contactOut,
OnRayPrecastAction preFilter, const dgBody* const body,
void* const userData) const
{
const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];
if (!m_rootNode)
{
return dgFloat32(1.2f);
}
dgInt32 stack = 1;
stackPool[0] = m_rootNode;
dgFloat32 maxParam = dgFloat32(1.2f);
//int xxx = 0;
dgFastRayTest ray(localP0, localP1);
while (stack)
{
stack--;
const dgNode* const me = stackPool[stack];
//xxx ++;
if (ray.BoxTest(me->m_minBox, me->m_maxBox))
{
if (!me->m_left)
{
_ASSERTE(!me->m_right);
dgContactPoint tmpContactOut;
const dgProxy* const proxy = (dgProxy*) me;
dgVector l0(proxy->m_matrix.UntransformVector(localP0));
dgVector l1(proxy->m_matrix.UntransformVector(localP1));
dgFloat32 param = proxy->m_shape->RayCast(l0, l1, tmpContactOut,
preFilter, body, userData);
_ASSERTE(param >= dgFloat32 (0.0f));
if (param < maxParam)
{
contactOut.m_normal = proxy->m_matrix.RotateVector(
tmpContactOut.m_normal);
maxParam = param;
ray.Reset(maxParam);
}
}
else
{
_ASSERTE(me->m_left);
_ASSERTE(stack < dgInt32 (sizeof (stackPool) / sizeof (dgNode*)));
stackPool[stack] = me->m_left;
stack++;
_ASSERTE(me->m_right);
_ASSERTE(stack < dgInt32 (sizeof (stackPool) / sizeof (dgNode*)));
stackPool[stack] = me->m_right;
stack++;
}
}
}
return maxParam;
}
dgInt32 AddFilterFace(dgUnsigned32 count, dgInt32* const pool)
{
BeginFace();
_ASSERTE(count);
bool reduction = true;
while (reduction && !AddFace(dgInt32(count), pool))
{
reduction = false;
if (count > 3)
{
for (dgUnsigned32 i = 0; i < count; i++)
{
for (dgUnsigned32 j = i + 1; j < count; j++)
{
if (pool[j] == pool[i])
{
for (i = j; i < count - 1; i++)
{
pool[i] = pool[i + 1];
}
count--;
i = count;
reduction = true;
break;
}
}
}
}
}
EndFace();
_ASSERTE(reduction);
return reduction ? dgInt32(count) : 0;
}
FastRayTest::FastRayTest(const dgVector& l0, const dgVector& l1)
:m_p0 (l0), m_p1(l1), m_diff (l1 - l0)
,m_minT(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
,m_maxT(dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f), dgFloat32 (1.0f))
,m_tolerance (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR)
,m_zero(dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f))
{
m_diff.m_w = dgFloat32 (0.0f);
m_isParallel[0] = (dgAbsf (m_diff.m_x) > dgFloat32 (1.0e-8f)) ? 0 : dgInt32 (0xffffffff);
m_isParallel[1] = (dgAbsf (m_diff.m_y) > dgFloat32 (1.0e-8f)) ? 0 : dgInt32 (0xffffffff);
m_isParallel[2] = (dgAbsf (m_diff.m_z) > dgFloat32 (1.0e-8f)) ? 0 : dgInt32 (0xffffffff);
m_isParallel[3] = 0;
m_dpInv.m_x = (!m_isParallel[0]) ? (dgFloat32 (1.0f) / m_diff.m_x) : dgFloat32 (1.0e20f);
m_dpInv.m_y = (!m_isParallel[1]) ? (dgFloat32 (1.0f) / m_diff.m_y) : dgFloat32 (1.0e20f);
m_dpInv.m_z = (!m_isParallel[2]) ? (dgFloat32 (1.0f) / m_diff.m_z) : dgFloat32 (1.0e20f);
m_dpInv.m_w = dgFloat32 (0.0f);
m_dpBaseInv = m_dpInv;
m_ray_xxxx = simd_128(m_diff.m_x);
m_ray_yyyy = simd_128(m_diff.m_y);
m_ray_zzzz = simd_128(m_diff.m_z);
m_dirError = -dgFloat32 (0.0175f) * dgSqrt (m_diff % m_diff);
// tollerance = simd_set (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR);
// m_tolerance = dgVector (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, dgFloat32 (0.0f));
}
dgCollisionConvexHull::dgCollisionConvexHull(dgWorld* const world, dgDeserialize deserialization, void* const userData)
:dgCollisionConvex (world, deserialization, userData)
{
m_rtti |= dgCollisionConvexHull_RTTI;
deserialization (userData, &m_vertexCount, sizeof (dgInt32));
deserialization (userData, &m_vertexCount, sizeof (dgInt32));
deserialization (userData, &m_faceCount, sizeof (dgInt32));
deserialization (userData, &m_edgeCount, sizeof (dgInt32));
deserialization (userData, &m_boundPlanesCount, sizeof (dgInt32));
deserialization (userData, &m_destructionImpulse, sizeof (dgFloat32));
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge)));
m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *)));
deserialization (userData, m_vertex, m_vertexCount * sizeof (dgVector));
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgInt32 serialization[4];
deserialization (userData, serialization, sizeof (serialization));
m_simplex[i].m_vertex = serialization[0];
m_simplex[i].m_twin = m_simplex + serialization[1];
m_simplex[i].m_next = m_simplex + serialization[2];
m_simplex[i].m_prev = m_simplex + serialization[3];
}
for (dgInt32 i = 0; i < m_faceCount; i ++) {
dgInt32 faceOffset;
deserialization (userData, &faceOffset, sizeof (dgInt32));
m_faceArray[i] = m_simplex + faceOffset;
}
SetVolumeAndCG ();
}
void dgCollisionConvexHull::Serialize(dgSerialize callback, void* const userData) const
{
SerializeLow(callback, userData);
callback (userData, &m_vertexCount, sizeof (dgInt32));
callback (userData, &m_vertexCount, sizeof (dgInt32));
callback (userData, &m_faceCount, sizeof (dgInt32));
callback (userData, &m_edgeCount, sizeof (dgInt32));
callback (userData, &m_boundPlanesCount, sizeof (dgInt32));
callback (userData, &m_destructionImpulse, sizeof (dgFloat32));
callback (userData, m_vertex, m_vertexCount * sizeof (dgVector));
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgInt32 serialization[4];
serialization[0] = m_simplex[i].m_vertex;
serialization[1] = dgInt32 (m_simplex[i].m_twin - m_simplex);
serialization[2] = dgInt32 (m_simplex[i].m_next - m_simplex);
serialization[3] = dgInt32 (m_simplex[i].m_prev - m_simplex);
callback (userData, serialization, sizeof (serialization));
}
for (dgInt32 i = 0; i < m_faceCount; i ++) {
dgInt32 faceOffset;
faceOffset = dgInt32 (m_faceArray[i] - m_simplex);
callback (userData, &faceOffset, sizeof (dgInt32));
}
}
bool dgCollisionConvexHull::OOBBTest (const dgMatrix& matrix, const dgCollisionConvex* const shape, void* const cacheOrder) const
{
bool ret;
_ASSERTE (cacheOrder);
ret = dgCollisionConvex::OOBBTest (matrix, shape, cacheOrder);
if (ret) {
const dgConvexSimplexEdge* const* faceArray = m_faceArray;
dgCollisionBoundPlaneCache* const cache = (dgCollisionBoundPlaneCache*)cacheOrder;
for (dgInt32 i = 0; i < dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)); i ++) {
dgFloat32 dist;
const dgPlane& plane = cache->m_planes[i];
if ((plane % plane) > dgFloat32 (0.0f)) {
dgVector dir (matrix.UnrotateVector(plane.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
dist = plane.Evalue (p);
if (dist > dgFloat32 (0.1f)){
return false;
}
}
}
for (dgInt32 i = 0; i < m_boundPlanesCount; i ++) {
dgInt32 i0;
dgInt32 i1;
dgInt32 i2;
dgFloat32 dist;
const dgConvexSimplexEdge* const face = faceArray[i];
i0 = face->m_prev->m_vertex;
i1 = face->m_vertex;
i2 = face->m_next->m_vertex;
const dgVector& p0 = m_vertex[i0];
dgVector normal ((m_vertex[i1] - p0) * (m_vertex[i2] - p0));
normal = normal.Scale (dgFloat32 (1.0f) / dgSqrt (normal % normal));
dgVector dir (matrix.UnrotateVector(normal.Scale (-1.0f)));
dir.m_w = dgFloat32 (0.0f);
dgVector p (matrix.TransformVector (shape->SupportVertex(dir)));
//_ASSERTE ((normal % (m_boxOrigin - p0)) < 0.0f);
dist = normal % (p - p0);
if (dist > dgFloat32 (0.1f)){
for (dgInt32 j = 0; j < (dgInt32 (sizeof (cache->m_planes) / sizeof (dgPlane)) - 1); j ++) {
cache->m_planes[j + 1] = cache->m_planes[j];
}
cache->m_planes[1] = dgPlane (normal, - (normal % p0));
return false;
}
}
}
return ret;
}
dgBody* dgWorld::CreateBody(dgCollision* const collision, const dgMatrix& matrix)
{
dgBody* body;
_ASSERTE (collision);
body = new (m_allocator) dgBody();
_ASSERTE ((sizeof (dgBody) & 0xf) == 0);
_ASSERTE ((dgUnsigned64 (body) & 0xf) == 0);
memset (body, 0, sizeof (dgBody));
// m_bodiesCount ++;
m_bodiesUniqueID ++;
body->m_world = this;
body->m_freeze = false;
body->m_sleeping = false;
body->m_autoSleep = true;
body->m_isInWorld = true;
body->m_equilibrium = false;
body->m_continueCollisionMode = false;
body->m_collideWithLinkedBodies = true;
body->m_solverInContinueCollision = false;
body->m_spawnnedFromCallback = dgUnsigned32 (m_inUpdate ? true : false);
body->m_uniqueID = dgInt32 (m_bodiesUniqueID);
dgBodyMasterList::AddBody(body);
// dgBodyActiveList___::AddBody(body);
// _ASSERTE (body->m_activeNode);
// body->m_freezeAccel2 = m_freezeAccel2;
// body->m_freezeAlpha2 = m_freezeAlpha2;
// body->m_freezeSpeed2 = m_freezeSpeed2;
// body->m_freezeOmega2 = m_freezeOmega2;
body->SetCentreOfMass (dgVector (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f)));
body->SetLinearDamping (dgFloat32 (0.1045f)) ;
body->SetAngularDamping (dgVector (dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.1045f), dgFloat32 (0.0f)));
body->AttachCollision(collision);
body->m_bodyGroupId = dgInt32 (m_defualtBodyGroupID);
body->SetMassMatrix (DG_INFINITE_MASS * dgFloat32 (2.0f), DG_INFINITE_MASS, DG_INFINITE_MASS, DG_INFINITE_MASS);
dgBroadPhaseCollision::Add (body);
//body->SetMatrix (dgGetIdentityMatrix());
body->SetMatrix (matrix);
body->m_invWorldInertiaMatrix[3][3] = dgFloat32 (1.0f);
return body;
}
void dgCollision::SerializeLow (dgSerialize callback, void* const userData) const
{
dgInt32 signature[4];
signature[0] = dgInt32 (GetSignature ());
signature[1] = GetCollisionPrimityType ();
signature[2] = dgInt32 (SetUserDataID());
signature[3] = 0;
callback (userData, &signature, sizeof (signature));
callback (userData, &m_offset, sizeof (dgMatrix));
}
dgCollisionConvexHull::dgCollisionConvexHull(dgWorld* const world, dgDeserialize deserialization, void* const userData, dgInt32 revisionNumber)
:dgCollisionConvex (world, deserialization, userData, revisionNumber)
,m_faceCount (0)
,m_supportTreeCount (0)
,m_faceArray (NULL)
,m_vertexToEdgeMapping(NULL)
,m_supportTree (NULL)
{
m_rtti |= dgCollisionConvexHull_RTTI;
deserialization (userData, &m_vertexCount, sizeof (dgInt32));
deserialization (userData, &m_vertexCount, sizeof (dgInt32));
deserialization (userData, &m_faceCount, sizeof (dgInt32));
deserialization (userData, &m_edgeCount, sizeof (dgInt32));
deserialization (userData, &m_supportTreeCount, sizeof (dgInt32));
m_vertex = (dgVector*) m_allocator->Malloc (dgInt32 (m_vertexCount * sizeof (dgVector)));
m_simplex = (dgConvexSimplexEdge*) m_allocator->Malloc (dgInt32 (m_edgeCount * sizeof (dgConvexSimplexEdge)));
m_faceArray = (dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_faceCount * sizeof(dgConvexSimplexEdge *)));
m_vertexToEdgeMapping = (const dgConvexSimplexEdge **) m_allocator->Malloc(dgInt32 (m_vertexCount * sizeof(dgConvexSimplexEdge *)));
if (m_supportTreeCount) {
m_supportTree = (dgConvexBox *) m_allocator->Malloc(dgInt32 (m_supportTreeCount * sizeof(dgConvexBox)));
deserialization (userData, m_supportTree, m_supportTreeCount * sizeof(dgConvexBox));
}
deserialization (userData, m_vertex, m_vertexCount * sizeof (dgVector));
for (dgInt32 i = 0; i < m_edgeCount; i ++) {
dgInt32 serialization[4];
deserialization (userData, serialization, sizeof (serialization));
m_simplex[i].m_vertex = serialization[0];
m_simplex[i].m_twin = m_simplex + serialization[1];
m_simplex[i].m_next = m_simplex + serialization[2];
m_simplex[i].m_prev = m_simplex + serialization[3];
}
for (dgInt32 i = 0; i < m_faceCount; i ++) {
dgInt32 faceOffset;
deserialization (userData, &faceOffset, sizeof (dgInt32));
m_faceArray[i] = m_simplex + faceOffset;
}
for (dgInt32 i = 0; i < m_vertexCount; i ++) {
dgInt32 faceOffset;
deserialization (userData, &faceOffset, sizeof (dgInt32));
m_vertexToEdgeMapping[i] = m_simplex + faceOffset;
}
SetVolumeAndCG ();
}
void dgCollisionScene::CollidePairSimd(
dgCollidingPairCollector::dgPair* const pair,
dgCollisionParamProxy& proxy) const
{
const dgNode *stackPool[DG_SCENE_MAX_STACK_DEPTH];
_ASSERTE(pair->m_body1->GetCollision() == this);
_ASSERTE(
pair->m_body1->GetCollision()->IsType(dgCollision::dgCollisionScene_RTTI));
dgVector p0;
dgVector p1;
_ASSERTE(m_world == pair->m_body1->GetWorld());
dgMatrix matrix(pair->m_body0->m_matrix * pair->m_body1->m_matrix.Inverse());
pair->m_body0->GetCollision()->CalcAABBSimd(matrix, p0, p1);
dgInt32 stack = 1;
stackPool[0] = m_rootNode;
while (stack)
{
stack--;
const dgNode* const me = stackPool[stack];
if (dgOverlapTestSimd(me->m_minBox, me->m_maxBox, p0, p1))
{
if (!me->m_left)
{
_ASSERTE(!me->m_right);
const dgProxy* const sceneProxy = (dgProxy*) me;
m_world->SceneContactsSimd(*sceneProxy, pair, proxy);
}
else
{
_ASSERTE(me->m_left);
_ASSERTE(stack < dgInt32 (sizeof (stackPool) / sizeof (dgNode*)));
stackPool[stack] = me->m_left;
stack++;
_ASSERTE(me->m_right);
_ASSERTE(stack < dgInt32 (sizeof (stackPool) / sizeof (dgNode*)));
stackPool[stack] = me->m_right;
stack++;
}
}
}
}
void NewtonUserJoint::SetSpringDamperAcceleration (dFloat spring, dFloat damper)
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF))) {
dgBilateralConstraint::SetSpringDamperAcceleration (index, *m_param, spring, damper);
}
}
void dgSolver::CalculateBodyForce(dgInt32 threadID)
{
const dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
const dgBodyJacobianPair* const bodyJacobiansPairs = m_bodyJacobiansPairs;
dgSoaFloat* const internalForces = (dgSoaFloat*)&m_world->GetSolverMemory().m_internalForcesBuffer[0];
const dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0];
const dgSoaFloat* const leftHandSide = (dgSoaFloat*)&m_world->GetSolverMemory().m_leftHandSizeBuffer[0].m_Jt.m_jacobianM0;
const dgInt32 step = m_threadCounts;;
const dgInt32 bodyCount = m_cluster->m_bodyCount;
for (dgInt32 i = threadID; i < bodyCount; i += step) {
dgSoaFloat forceAcc (m_soaZero);
const dgBodyProxy* const startJoints = &bodyProxyArray[i];
const dgInt32 jointsCount = dgInt32(startJoints->m_weight);
const dgBodyJacobianPair* const jointsStart = &bodyJacobiansPairs[startJoints->m_jointStart];
for (dgInt32 j = 0; j < jointsCount; j++) {
const dgInt32 rowsCount = jointsStart[j].m_rowCount - 2;
const dgSoaFloat* const lhs = &leftHandSide[jointsStart[j].m_rowStart];
const dgRightHandSide* const rhs = &rightHandSide[jointsStart[j].m_righHandStart];
for (dgInt32 k = 0; k < rowsCount; k += 2) {
forceAcc = forceAcc.MulAdd(lhs[(k + 0) * 4], dgSoaFloat(rhs[k + 0].m_force));
forceAcc = forceAcc.MulAdd(lhs[(k + 1) * 4], dgSoaFloat(rhs[k + 1].m_force));
}
if (jointsStart[j].m_rowCount & 1) {
const dgInt32 k = jointsStart[j].m_rowCount - 1;
forceAcc = forceAcc.MulAdd(lhs[k * 4], dgSoaFloat(rhs[k].m_force));
}
}
internalForces[i] = forceAcc * dgSoaFloat(startJoints->m_invWeight);
}
}
bool dgApi dgPointToPolygonDistance (const dgVector& p, const dgFloat32* const polygon, dgInt32 strideInBytes,
const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 bailDistance, dgVector& out)
{
_ASSERTE (0);
dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
dgInt32 i0 = indexArray[0] * stride;
dgInt32 i1 = indexArray[1] * stride;
const dgVector v0 (&polygon[i0]);
dgVector v1 (&polygon[i1]);
dgVector closestPoint (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgFloat32 minDist = dgFloat32 (1.0e20f);
for (dgInt32 i = 2; i < indexCount; i ++) {
dgInt32 i2 = indexArray[i] * stride;
const dgVector v2 (&polygon[i2]);
const dgVector q (dgPointToTriangleDistance (p, v0, v1, v2));
const dgVector error (q - p);
dgFloat32 dist = error % error;
if (dist < minDist) {
minDist = dist;
closestPoint = q;
}
v1 = v2;
}
if (minDist > (bailDistance * bailDistance)) {
return false;
}
out = closestPoint;
return true;
}
void dgWorld::OnBodySerializeToFile(dgBody& body, void* const userData, dgSerialize serializeCallback, void* const serializeHandle)
{
const char* const bodyIndentification = "NewtonGravityBody\0\0\0\0";
int size = (dgInt32(strlen(bodyIndentification)) + 3) & -4;
serializeCallback(serializeHandle, &size, sizeof (size));
serializeCallback(serializeHandle, bodyIndentification, size);
}
dgFloat32 NewtonUserJoint::GetRowForce (dgInt32 row) const
{
dgFloat32 force = 0.0f;
if ((row >= 0) && (row < dgInt32 (m_maxDOF)))
force = m_forceArray[row].m_force;
return force;
}
void dgSolver::CalculateJointForces(const dgBodyCluster& cluster, dgBodyInfo* const bodyArray, dgJointInfo* const jointArray, dgFloat32 timestep)
{
m_cluster = &cluster;
m_bodyArray = bodyArray;
m_jointArray = jointArray;
m_timestep = timestep;
m_invTimestep = (timestep > dgFloat32(0.0f)) ? dgFloat32(1.0f) / timestep : dgFloat32(0.0f);
m_invStepRK = dgFloat32 (0.25f);
m_timestepRK = m_timestep * m_invStepRK;
m_invTimestepRK = m_invTimestep * dgFloat32 (4.0f);
m_threadCounts = m_world->GetThreadCount();
m_solverPasses = m_world->GetSolverIterations();
dgInt32 mask = -dgInt32(DG_SOA_WORD_GROUP_SIZE - 1);
m_jointCount = ((m_cluster->m_jointCount + DG_SOA_WORD_GROUP_SIZE - 1) & mask) / DG_SOA_WORD_GROUP_SIZE;
m_bodyProxyArray = dgAlloca(dgBodyProxy, cluster.m_bodyCount);
m_bodyJacobiansPairs = dgAlloca(dgBodyJacobianPair, cluster.m_jointCount * 2);
m_soaRowStart = dgAlloca(dgInt32, cluster.m_jointCount / DG_SOA_WORD_GROUP_SIZE + 1);
InitWeights();
InitBodyArray();
InitJacobianMatrix();
CalculateForces();
}
void NewtonUserJoint::SetAcceleration (dgFloat32 acceleration)
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF))) {
SetMotorAcceleration (index, acceleration, *m_param);
}
}
void dgSphere::SetDimensions(const dgFloat32 vertex[], dgInt32 strideInBytes,
const dgInt32 triangles[], dgInt32 indexCount, const dgMatrix *basis)
{
dgVector eigen;
dgVector scaleVector(dgFloat32(1.0f), dgFloat32(1.0f), dgFloat32(1.0f),
dgFloat32(0.0f));
if (indexCount < 3)
{
return;
}
dgInt32 stride = dgInt32(strideInBytes / sizeof(dgFloat32));
if (!basis)
{
InternalSphere::Statistics(*this, eigen, scaleVector, vertex, triangles,
indexCount, stride);
dgInt32 k = 0;
for (dgInt32 i = 0; i < 3; i++)
{
if (k >= 6)
{
break;
}
for (dgInt32 j = i + 1; j < 3; j++)
{
dgFloat32 aspect = InternalSphere::AspectRatio(eigen[i], eigen[j]);
if (aspect > dgFloat32(0.9f))
{
scaleVector[i] *= dgFloat32(2.0f);
InternalSphere::Statistics(*this, eigen, scaleVector, vertex,
triangles, indexCount, stride);
k++;
i = -1;
break;
}
}
}
}
else
{
*this = *basis;
}
dgVector min;
dgVector max;
InternalSphere::BoundingBox(*this, vertex, stride, triangles, indexCount, min,
max);
dgVector massCenter(max + min);
massCenter = massCenter.Scale(dgFloat32(0.5f));
m_posit = TransformVector(massCenter);
dgVector dim(max - min);
dim = dim.Scale(dgFloat32(0.5f));
SetDimensions(dim.m_x, dim.m_y, dim.m_z);
}
dgFloat32 NewtonUserJoint::CalculateZeroMotorAcceleration() const
{
dgInt32 index = m_rows - 1;
dgFloat32 accel = dgFloat32 (0.0f);
if ((index >= 0) && (index < dgInt32 (m_maxDOF)))
accel = CalculateMotorAcceleration (index, *m_param);
return accel;
}
void dgCollidingPairCollector::Init ()
{
// m_world = me;
dgWorld* const world = (dgWorld*) this;
m_count = 0;
m_maxSize = dgInt32 (world->m_pairMemoryBufferSizeInBytes / sizeof (dgPair));
m_pairs = (dgPair*) world->m_pairMemoryBuffer ;
}
dFloat NewtonUserJoint::GetAcceleration () const
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF))) {
return GetRowAcceleration (index, *m_param);
}
return 0.0f;
}
void dgCollision::GetCollisionInfo(dgCollisionInfo* info) const
{
// memset (info, 0, sizeof (dgCollisionInfo));
info->m_offsetMatrix = dgGetIdentityMatrix();
info->m_collisionType = m_collsionId;
info->m_refCount = GetRefCount();
info->m_userDadaID = dgInt32 (SetUserDataID());
}
dgKinematicBody* dgWorld::CreateKinematicBody (dgCollisionInstance* const collision, const dgMatrix& matrix)
{
dgKinematicBody* const body = new (m_allocator) dgKinematicBody();
dgAssert (dgInt32 (sizeof (dgBody) & 0xf) == 0);
dgAssert ((dgUnsigned64 (body) & 0xf) == 0);
InitBody (body, collision, matrix);
return body;
}
dgFloat32 NewtonUserJoint::GetInverseDynamicsAcceleration() const
{
dgInt32 index = m_rows - 1;
dgFloat32 accel = dgFloat32(0.0f);
if ((index >= 0) && (index < dgInt32(m_maxDOF))) {
accel = GetInverseDynamicAcceleration(index);
}
return accel;
}
void NewtonUserJoint::SetSpringDamperAcceleration (dFloat springK, dFloat damperD)
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF)))
{
dgFloat32 accel = CalculateSpringDamperAcceleration (index, *m_param, m_lastJointAngle, m_lastPosit0, m_lastPosit1, springK, damperD);
SetMotorAcceleration (index, accel, *m_param);
}
}
void NewtonUserJoint::AddGeneralRowJacobian (const dgFloat32 * const jacobian0, const dgFloat32 * const jacobian1)
{
m_lastPosit0 = dgVector (dgFloat32 (0.0f));
m_lastPosit1 = dgVector (dgFloat32 (0.0f));
m_lastJointAngle = 0.0f;
SetJacobianDerivative (m_rows, *m_param, jacobian0, jacobian1, &m_forceArray[m_rows]);
m_rows ++;
dgAssert (m_rows <= dgInt32 (m_maxDOF));
}
void NewtonUserJoint::AddAngularRowJacobian (const dgVector & dir, dgFloat32 relAngle)
{
m_lastPosit0 = dgVector (dgFloat32 (0.0f));
m_lastPosit1 = dgVector (dgFloat32 (0.0f));
m_lastJointAngle = relAngle;
CalculateAngularDerivative (m_rows, *m_param, dir, m_stiffness, relAngle, &m_forceArray[m_rows]);
m_rows ++;
dgAssert (m_rows <= dgInt32 (m_maxDOF));
}
void NewtonUserJoint::AddLinearRowJacobian (const dgVector& pivot0, const dgVector& pivot1, const dgVector& dir)
{
dgPointParam pointData;
InitPointParam (pointData, m_stiffness, pivot0, pivot1);
CalculatePointDerivative (m_rows, *m_param, dir, pointData, &m_forceArray[m_rows]);
m_rows ++;
dgAssert (m_rows <= dgInt32 (m_maxDOF));
}
void NewtonUserJoint::SetRowStiffness (dgFloat32 stiffness)
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF))) {
stiffness = dgFloat32 (1.0f) - dgClamp (stiffness, dgFloat32(0.0f), dgFloat32(1.0f));
stiffness = -dgFloat32 (1.0f) - stiffness / DG_PSD_DAMP_TOL;
m_param->m_jointStiffness[index] = stiffness;
}
}
