void dgCollisionSphere::Init (dgFloat32 radius, dgMemoryAllocator* allocator)
{
m_rtti |= dgCollisionSphere_RTTI;
m_radius = dgMax (dgAbs (radius), D_MIN_CONVEX_SHAPE_SIZE);
m_edgeCount = DG_SPHERE_EDGE_COUNT;
m_vertexCount = DG_SPHERE_VERTEX_COUNT;
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgInt32 indexList[256];
dgVector tmpVectex[256];
dgVector p0 ( dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p1 (-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p2 ( dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p3 ( dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector p4 ( dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f));
dgVector p5 ( dgFloat32 (0.0f), dgFloat32 (0.0f),-dgFloat32 (1.0f), dgFloat32 (0.0f));
dgInt32 index = 1;
dgInt32 count = 0;
TesselateTriangle (index, p4, p0, p2, count, tmpVectex);
TesselateTriangle (index, p4, p2, p1, count, tmpVectex);
TesselateTriangle (index, p4, p1, p3, count, tmpVectex);
TesselateTriangle (index, p4, p3, p0, count, tmpVectex);
TesselateTriangle (index, p5, p2, p0, count, tmpVectex);
TesselateTriangle (index, p5, p1, p2, count, tmpVectex);
TesselateTriangle (index, p5, p3, p1, count, tmpVectex);
TesselateTriangle (index, p5, p0, p3, count, tmpVectex);
//dgAssert (count == EDGE_COUNT);
dgInt32 vertexCount = dgVertexListToIndexList (&tmpVectex[0].m_x, sizeof (dgVector), 3 * sizeof (dgFloat32), 0, count, indexList, 0.001f);
dgAssert (vertexCount == DG_SPHERE_VERTEX_COUNT);
for (dgInt32 i = 0; i < vertexCount; i ++) {
m_unitSphere[i] = tmpVectex[i];
}
dgPolyhedra polyhedra(m_allocator);
polyhedra.BeginFace();
for (dgInt32 i = 0; i < count; i += 3) {
#ifdef _DEBUG
dgEdge* const edge = polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
dgAssert (edge);
#else
polyhedra.AddFace (indexList[i], indexList[i + 1], indexList[i + 2]);
#endif
}
polyhedra.EndFace();
dgUnsigned64 i1 = 0;
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
edge->m_userData = i1;
i1 ++;
}
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];
}
}
for (dgInt32 i = 0; i < DG_SPHERE_VERTEX_COUNT; i ++) {
m_vertex[i] = m_unitSphere[i].Scale (m_radius);
}
m_shapeRefCount ++;
dgCollisionConvex::m_simplex = m_edgeArray;
SetVolumeAndCG ();
}
void dgSolver::CalculateJointsForce(dgInt32 threadID)
{
const dgInt32* const soaRowStart = m_soaRowStart;
const dgBodyInfo* const bodyArray = m_bodyArray;
dgSoaMatrixElement* const massMatrix = &m_massMatrix[0];
dgRightHandSide* const rightHandSide = &m_world->GetSolverMemory().m_righHandSizeBuffer[0];
dgJacobian* const internalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[0];
dgFloat32 accNorm = dgFloat32(0.0f);
const dgInt32 step = m_threadCounts;
const dgInt32 jointCount = m_jointCount;
for (dgInt32 i = threadID; i < jointCount; i += step) {
const dgInt32 rowStart = soaRowStart[i];
dgJointInfo* const jointInfo = &m_jointArray[i * DG_SOA_WORD_GROUP_SIZE];
bool isSleeping = true;
dgFloat32 accel2 = dgFloat32(0.0f);
for (dgInt32 j = 0; (j < DG_SOA_WORD_GROUP_SIZE) && isSleeping; j++) {
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
const dgBody* const body0 = bodyArray[m0].m_body;
const dgBody* const body1 = bodyArray[m1].m_body;
isSleeping &= body0->m_resting;
isSleeping &= body1->m_resting;
}
if (!isSleeping) {
accel2 = CalculateJointForce(jointInfo, &massMatrix[rowStart], internalForces);
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgInt32 const rowCount = joint->m_pairCount;
dgInt32 const rowStartBase = joint->m_pairStart;
for (dgInt32 k = 0; k < rowCount; k++) {
const dgSoaMatrixElement* const row = &massMatrix[rowStart + k];
rightHandSide[k + rowStartBase].m_force = row->m_force[j];
rightHandSide[k + rowStartBase].m_maxImpact = dgMax(dgAbs(row->m_force[j]), rightHandSide[k + rowStartBase].m_maxImpact);
}
}
}
}
dgSoaVector6 forceM0;
dgSoaVector6 forceM1;
forceM0.m_linear.m_x = m_soaZero;
forceM0.m_linear.m_y = m_soaZero;
forceM0.m_linear.m_z = m_soaZero;
forceM0.m_angular.m_x = m_soaZero;
forceM0.m_angular.m_y = m_soaZero;
forceM0.m_angular.m_z = m_soaZero;
forceM1.m_linear.m_x = m_soaZero;
forceM1.m_linear.m_y = m_soaZero;
forceM1.m_linear.m_z = m_soaZero;
forceM1.m_angular.m_x = m_soaZero;
forceM1.m_angular.m_y = m_soaZero;
forceM1.m_angular.m_z = m_soaZero;
const dgInt32 rowsCount = jointInfo->m_pairCount;
for (dgInt32 j = 0; j < rowsCount; j++) {
dgSoaMatrixElement* const row = &massMatrix[rowStart + j];
dgSoaFloat f(row->m_force);
forceM0.m_linear.m_x = forceM0.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_x, f);
forceM0.m_linear.m_y = forceM0.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_y, f);
forceM0.m_linear.m_z = forceM0.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_linear.m_z, f);
forceM0.m_angular.m_x = forceM0.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_x, f);
forceM0.m_angular.m_y = forceM0.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_y, f);
forceM0.m_angular.m_z = forceM0.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM0.m_angular.m_z, f);
forceM1.m_linear.m_x = forceM1.m_linear.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_x, f);
forceM1.m_linear.m_y = forceM1.m_linear.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_y, f);
forceM1.m_linear.m_z = forceM1.m_linear.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_linear.m_z, f);
forceM1.m_angular.m_x = forceM1.m_angular.m_x.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_x, f);
forceM1.m_angular.m_y = forceM1.m_angular.m_y.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_y, f);
forceM1.m_angular.m_z = forceM1.m_angular.m_z.MulAdd(row->m_Jt.m_jacobianM1.m_angular.m_z, f);
}
dgBodyProxy* const bodyProxyArray = m_bodyProxyArray;
dgJacobian* const tempInternalForces = &m_world->GetSolverMemory().m_internalForcesBuffer[m_cluster->m_bodyCount];
for (dgInt32 j = 0; j < DG_SOA_WORD_GROUP_SIZE; j++) {
const dgJointInfo* const joint = &jointInfo[j];
if (joint->m_joint) {
dgJacobian m_body0Force;
dgJacobian m_body1Force;
m_body0Force.m_linear = dgVector(forceM0.m_linear.m_x[j], forceM0.m_linear.m_y[j], forceM0.m_linear.m_z[j], dgFloat32(0.0f));
m_body0Force.m_angular = dgVector(forceM0.m_angular.m_x[j], forceM0.m_angular.m_y[j], forceM0.m_angular.m_z[j], dgFloat32(0.0f));
m_body1Force.m_linear = dgVector(forceM1.m_linear.m_x[j], forceM1.m_linear.m_y[j], forceM1.m_linear.m_z[j], dgFloat32(0.0f));
m_body1Force.m_angular = dgVector(forceM1.m_angular.m_x[j], forceM1.m_angular.m_y[j], forceM1.m_angular.m_z[j], dgFloat32(0.0f));
const dgInt32 m0 = jointInfo[j].m_m0;
const dgInt32 m1 = jointInfo[j].m_m1;
if (m0) {
dgScopeSpinPause lock(&bodyProxyArray[m0].m_lock);
tempInternalForces[m0].m_linear += m_body0Force.m_linear;
tempInternalForces[m0].m_angular += m_body0Force.m_angular;
}
if (m1) {
dgScopeSpinPause lock(&bodyProxyArray[m1].m_lock);
tempInternalForces[m1].m_linear += m_body1Force.m_linear;
tempInternalForces[m1].m_angular += m_body1Force.m_angular;
}
}
}
accNorm += accel2;
}
m_accelNorm[threadID] = accNorm;
}
dgFloat32 dgCollisionChamferCylinder::RayCast(const dgVector& q0, const dgVector& q1, dgFloat32 maxT, dgContactPoint& contactOut, const dgBody* const body, void* const userData, OnRayPrecastAction preFilter) const
{
if (q0.m_x > m_height) {
if (q1.m_x < m_height) {
dgFloat32 t1 = (m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
return t1;
}
}
}
if (q0.m_x < -m_height) {
if (q1.m_x > -m_height) {
dgFloat32 t1 = (-m_height - q0.m_x) / (q1.m_x - q0.m_x);
dgFloat32 y = q0.m_y + (q1.m_y - q0.m_y) * t1;
dgFloat32 z = q0.m_z + (q1.m_z - q0.m_z) * t1;
if ((y * y + z * z) < m_radius * m_radius) {
contactOut.m_normal = dgVector(dgFloat32(-1.0f), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
return t1;
}
}
}
dgVector dq((q1 - q0) & dgVector::m_triplexMask);
// avoid NaN as a result of a division by zero
if (dq.DotProduct(dq).GetScalar() <= 0.0f) {
return dgFloat32(1.2f);
}
//dgVector dir(dq * dq.InvMagSqrt());
dgVector dir(dq.Normalize());
if (dgAbs(dir.m_x) > 0.9999f) {
return dgCollisionConvex::RayCast(q0, q1, maxT, contactOut, body, NULL, NULL);
}
dgVector p0(q0 & dgVector::m_triplexMask);
dgVector p1(q1 & dgVector::m_triplexMask);
p0.m_x = dgFloat32 (0.0f);
p1.m_x = dgFloat32 (0.0f);
dgVector dp (p1 - p0);
dgFloat32 a = dp.DotProduct(dp).GetScalar();
dgFloat32 b = dgFloat32 (2.0f) * dp.DotProduct(p0).GetScalar();
dgFloat32 c = p0.DotProduct(p0).GetScalar() - m_radius * m_radius;
dgFloat32 disc = b * b - dgFloat32 (4.0f) * a * c;
if (disc >= dgFloat32 (0.0f)) {
disc = dgSqrt (disc);
dgVector origin0(p0 + dp.Scale ((-b + disc) / (dgFloat32 (2.0f) * a)));
dgVector origin1(p0 + dp.Scale ((-b - disc) / (dgFloat32 (2.0f) * a)));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
dgFloat32 t1 = dgRayCastSphere(q0, q1, origin1, m_height);
if(t1 < t0) {
t0 = t1;
origin0 = origin1;
}
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
//contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt();
contactOut.m_normal = contactOut.m_normal.Normalize();
return t0;
}
} else {
dgVector origin0 (dgPointToRayDistance (dgVector::m_zero, p0, p1));
origin0 = origin0.Scale(m_radius / dgSqrt(origin0.DotProduct(origin0).GetScalar()));
dgFloat32 t0 = dgRayCastSphere(q0, q1, origin0, m_height);
if ((t0 >= 0.0f) && (t0 <= 1.0f)) {
contactOut.m_normal = q0 + dq.Scale(t0) - origin0;
dgAssert(contactOut.m_normal.m_w == dgFloat32(0.0f));
//contactOut.m_normal = contactOut.m_normal * contactOut.m_normal.DotProduct(contactOut.m_normal).InvSqrt();
contactOut.m_normal = contactOut.m_normal.Normalize();
return t0;
}
}
return dgFloat32(1.2f);
}
void dgCollisionChamferCylinder::Init (dgFloat32 radius, dgFloat32 height)
{
m_rtti |= dgCollisionChamferCylinder_RTTI;
m_radius = dgMax (dgAbs (radius), D_MIN_CONVEX_SHAPE_SIZE);
m_height = dgMax (dgAbs (height * dgFloat32 (0.5f)), D_MIN_CONVEX_SHAPE_SIZE);
dgFloat32 sliceAngle = dgFloat32 (0.0f);
dgFloat32 sliceStep = dgPI / DG_CHAMFERCYLINDER_SLICES;
dgFloat32 breakStep = dgPI2 / DG_CHAMFERCYLINDER_BRAKES;
dgMatrix rot (dgPitchMatrix (breakStep));
dgInt32 index = 0;
for (dgInt32 j = 0; j <= DG_CHAMFERCYLINDER_SLICES; j ++) {
dgVector p0 (-m_height * dgCos(sliceAngle), dgFloat32 (0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32 (0.0f));
sliceAngle += sliceStep;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
m_vertex[index] = p0;
index ++;
p0 = rot.UnrotateVector (p0);
}
}
m_edgeCount = (4 * DG_CHAMFERCYLINDER_SLICES + 2)* DG_CHAMFERCYLINDER_BRAKES;
m_vertexCount = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1);
dgCollisionConvex::m_vertex = m_vertex;
if (!m_shapeRefCount) {
dgPolyhedra polyhedra(m_allocator);
dgInt32 wireframe[DG_CHAMFERCYLINDER_SLICES + 10];
for (dgInt32 i = 0; i < DG_MAX_CHAMFERCYLINDER_DIR_COUNT; i ++) {
dgMatrix matrix (dgPitchMatrix (dgFloat32 (dgPI2 * i) / DG_MAX_CHAMFERCYLINDER_DIR_COUNT));
m_shapesDirs[i] = matrix.RotateVector (dgVector (dgFloat32 (0.0f), dgFloat32 (1.0f), dgFloat32 (0.0f), dgFloat32 (0.0f)));
}
dgInt32 index0 = 0;
for (dgInt32 j = 0; j < DG_CHAMFERCYLINDER_SLICES; j ++) {
dgInt32 index1 = index0 + DG_CHAMFERCYLINDER_BRAKES - 1;
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[0] = index0;
wireframe[1] = index1;
wireframe[2] = index1 + DG_CHAMFERCYLINDER_BRAKES;
wireframe[3] = index0 + DG_CHAMFERCYLINDER_BRAKES;
index1 = index0;
index0 ++;
polyhedra.AddFace (4, wireframe);
}
}
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[i] = i;
}
polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, wireframe);
for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i ++) {
wireframe[i] = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1) - i - 1;
}
polyhedra.AddFace (DG_CHAMFERCYLINDER_BRAKES, 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 dgApi dgRayToRayDistance (const dgVector& ray_p0, const dgVector& ray_p1, const dgVector& ray_q0, const dgVector& ray_q1, dgVector& pOut, dgVector& qOut)
{
dgFloat32 sN;
dgFloat32 tN;
dgVector u (ray_p1 - ray_p0);
dgVector v (ray_q1 - ray_q0);
dgVector w (ray_p0 - ray_q0);
dgFloat32 a = u.DotProduct3(u);
dgFloat32 b = u.DotProduct3(v);
dgFloat32 c = v.DotProduct3(v);
dgFloat32 d = u.DotProduct3(w);
dgFloat32 e = v.DotProduct3(w);
dgFloat32 D = a*c - b*b;
dgFloat32 sD = D;
dgFloat32 tD = D;
// compute the line parameters of the two closest points
if (D < dgFloat32 (1.0e-8f)) {
sN = dgFloat32 (0.0f);
sD = dgFloat32 (1.0f);
tN = e;
tD = c;
} else {
// get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < dgFloat32 (0.0f)) {
// sc < 0 => the s=0 edge is visible
sN = dgFloat32 (0.0f);
tN = e;
tD = c;
} else if (sN > sD) {
// sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < dgFloat32 (0.0f)) {
// tc < 0 => the t=0 edge is visible
tN = dgFloat32 (0.0f);
// recompute sc for this edge
if (-d < dgFloat32 (0.0f)) {
sN = dgFloat32 (0.0f);
} else if (-d > a) {
sN = sD;
} else {
sN = -d;
sD = a;
}
} else if (tN > tD) {
// tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < dgFloat32 (0.0f)) {
sN = dgFloat32 (0.0f);
} else if ((-d + b) > a) {
sN = sD;
} else {
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
dgFloat32 sc = (dgAbs(sN) < dgFloat32(1.0e-8f) ? dgFloat32 (0.0f) : sN / sD);
dgFloat32 tc = (dgAbs(tN) < dgFloat32(1.0e-8f) ? dgFloat32 (0.0f) : tN / tD);
dgAssert (u.m_w == dgFloat32 (0.0f));
dgAssert (v.m_w == dgFloat32 (0.0f));
pOut = ray_p0 + u.Scale (sc);
qOut = ray_q0 + v.Scale (tc);
}
