void dgBilateralConstraint::SetSpringDamperAcceleration (dgInt32 index, dgContraintDescritor& desc, dgFloat32 spring, dgFloat32 damper)
{
if (desc.m_timestep > dgFloat32 (0.0f)) {
dgAssert (m_body1);
const dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
const dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;
dgVector veloc0 (m_body0->m_veloc);
dgVector omega0 (m_body0->m_omega);
dgVector veloc1 (m_body1->m_veloc);
dgVector omega1 (m_body1->m_omega);
//dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle;
dgFloat32 relPosit = desc.m_penetration[index];
dgFloat32 relVeloc = - (veloc0 % jacobian0.m_linear + veloc1 % jacobian1.m_linear + omega0 % jacobian0.m_angular + omega1 % jacobian1.m_angular);
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
dgFloat32 dt = desc.m_timestep;
dgFloat32 ks = dgAbsf (spring);
dgFloat32 kd = dgAbsf (damper);
dgFloat32 ksd = dt * ks;
dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
dgFloat32 den = dt * kd + dt * ksd;
dgFloat32 accel = num / (dgFloat32 (1.0f) + den);
// desc.m_jointStiffness[index] = - den / DG_PSD_DAMP_TOL ;
desc.m_jointStiffness[index] = - dgFloat32 (1.0f) - den / DG_PSD_DAMP_TOL;
SetMotorAcceleration (index, accel, desc);
}
}
dgInt32 dgCollisionTaperedCylinder::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgInt32 count;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
dgFloat32 magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
dgFloat32 cosAng = normal.m_y * magInv;
dgFloat32 sinAng = normal.m_z * magInv;
dgAssert (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,
origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f));
count = dgCollisionConvex::CalculatePlaneIntersection (normal1, origin1, contactsOut, normalSign);
for (dgInt32 i = 0; i < count; i ++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
} else {
count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut, normalSign);
}
return count;
}
void dgCollisionInstance::SetGlobalScale (const dgVector& scale)
{
if ((dgAbsf (scale[0] - scale[1]) < dgFloat32 (1.0e-4f)) && (dgAbsf (scale[0] - scale[2]) < dgFloat32 (1.0e-4f))) {
m_localMatrix.m_posit = m_localMatrix.m_posit.Scale3 (scale.m_x * m_invScale.m_x);
SetScale (scale);
} else {
// extract the original local matrix
dgMatrix localMatrix (m_aligmentMatrix * m_localMatrix);
// create a new scale matrix
localMatrix[0] = localMatrix[0].CompProduct4 (scale);
localMatrix[1] = localMatrix[1].CompProduct4 (scale);
localMatrix[2] = localMatrix[2].CompProduct4 (scale);
localMatrix[3] = localMatrix[3].CompProduct4 (scale);
localMatrix[3][3] = dgFloat32 (1.0f);
// decompose into to align * scale * local
localMatrix.PolarDecomposition (m_localMatrix, m_scale, m_aligmentMatrix);
m_localMatrix = m_aligmentMatrix * m_localMatrix;
m_aligmentMatrix = m_aligmentMatrix.Transpose();
bool isIdentity = true;
for (dgInt32 i = 0; i < 3; i ++) {
isIdentity &= dgAbsf (m_aligmentMatrix[i][i] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f);
isIdentity &= dgAbsf (m_aligmentMatrix[3][i]) < dgFloat32 (1.0e-5f);
}
m_scaleType = isIdentity ? m_nonUniform : m_global;
m_maxScale = dgMax(m_scale[0], m_scale[1], m_scale[2]);
m_invScale = dgVector (dgFloat32 (1.0f) / m_scale[0], dgFloat32 (1.0f) / m_scale[1], dgFloat32 (1.0f) / m_scale[2], dgFloat32 (0.0f));
}
}
dgFastRayTest::dgFastRayTest(const dgVector& l0, const dgVector& l1)
:m_p0 (l0), m_p1(l1), m_diff (l1 - l0)
,m_minT(float (0.0f), float (0.0f), float (0.0f), float (0.0f))
,m_maxT(float (1.0f), float (1.0f), float (1.0f), float (1.0f))
,m_tolerance (DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR, DG_RAY_TOL_ERROR)
,m_zero(float (0.0f), float (0.0f), float (0.0f), float (0.0f))
{
m_diff.m_w = float (0.0f);
m_isParallel[0] = (dgAbsf (m_diff.m_x) > float (1.0e-8f)) ? 0 : int32_t (0xffffffff);
m_isParallel[1] = (dgAbsf (m_diff.m_y) > float (1.0e-8f)) ? 0 : int32_t (0xffffffff);
m_isParallel[2] = (dgAbsf (m_diff.m_z) > float (1.0e-8f)) ? 0 : int32_t (0xffffffff);
m_isParallel[3] = 0;
m_dpInv.m_x = (!m_isParallel[0]) ? (float (1.0f) / m_diff.m_x) : float (1.0e20f);
m_dpInv.m_y = (!m_isParallel[1]) ? (float (1.0f) / m_diff.m_y) : float (1.0e20f);
m_dpInv.m_z = (!m_isParallel[2]) ? (float (1.0f) / m_diff.m_z) : float (1.0e20f);
m_dpInv.m_w = float (0.0f);
m_dpBaseInv = m_dpInv;
m_dirError = -float (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, float (0.0f));
}
dgMatrix dgMatrix::Symetric3by3Inverse () const
{
dgMatrix copy(*this);
dgMatrix inverse(dgGetIdentityMatrix());
for (dgInt32 i = 0; i < 3; i++) {
dgVector den(dgFloat32(1.0f) / copy[i][i]);
copy[i] = copy[i].CompProduct4(den);
inverse[i] = inverse[i].CompProduct4(den);
for (dgInt32 j = 0; j < 3; j++) {
if (j != i) {
dgVector pivot(copy[j][i]);
copy[j] -= copy[i].CompProduct4(pivot);
inverse[j] -= inverse[i].CompProduct4(pivot);
}
}
}
#ifdef _DEBUG
dgMatrix test(*this * inverse);
dgAssert(dgAbsf(test[0][0] - dgFloat32(1.0f)) < dgFloat32(0.01f));
dgAssert(dgAbsf(test[1][1] - dgFloat32(1.0f)) < dgFloat32(0.01f));
dgAssert(dgAbsf(test[2][2] - dgFloat32(1.0f)) < dgFloat32(0.01f));
#endif
return inverse;
}
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));
}
dgVector dgCollisionTaperedCylinder::ConvexConicSupporVertex (const dgVector& dir) const
{
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
dgFloat32 height = (m_height > DG_TAPED_CYLINDER_SKIN_PADDING) ? m_height - DG_TAPED_CYLINDER_SKIN_PADDING : m_height;
dgFloat32 radio0 = (m_radio0 > DG_TAPED_CYLINDER_SKIN_PADDING) ? m_radio0 - DG_TAPED_CYLINDER_SKIN_PADDING : m_radio0;
dgFloat32 radio1 = (m_radio1 > DG_TAPED_CYLINDER_SKIN_PADDING) ? m_radio1 - DG_TAPED_CYLINDER_SKIN_PADDING : m_radio1;
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
dgFloat32 y0 = radio0;
dgFloat32 z0 = dgFloat32 (0.0f);
dgFloat32 y1 = radio1;
dgFloat32 z1 = dgFloat32 (0.0f);
dgFloat32 mag2 = dir.m_y * dir.m_y + dir.m_z * dir.m_z;
if (mag2 > dgFloat32 (1.0e-12f)) {
mag2 = dgRsqrt(mag2);
y0 = dir.m_y * radio0 * mag2;
z0 = dir.m_z * radio0 * mag2;
y1 = dir.m_y * radio1 * mag2;
z1 = dir.m_z * radio1 * mag2;
}
dgVector p0 ( height, y0, z0, dgFloat32 (0.0f));
dgVector p1 (-height, y1, z1, dgFloat32 (0.0f));
dgFloat32 dist0 = dir % p0;
dgFloat32 dist1 = dir % p1;
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
dgUnsigned32 dgHingeConstraint::JacobianDerivative (dgContraintDescritor& params)
{
dgMatrix matrix0;
dgMatrix matrix1;
dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1));
m_angle = -angle.m_x;
dgAssert (dgAbsf (1.0f - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f));
dgAssert (dgAbsf (1.0f - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f));
dgAssert (dgAbsf (1.0f - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f));
const dgVector& dir0 = matrix0.m_front;
const dgVector& dir1 = matrix0.m_up;
const dgVector& dir2 = matrix0.m_right;
const dgVector& p0 = matrix0.m_posit;
const dgVector& p1 = matrix1.m_posit;
dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH));
// dgAssert (((p1 - p0) % (p1 - p0)) < 1.0e-2f);
dgPointParam pointDataP;
dgPointParam pointDataQ;
InitPointParam (pointDataP, m_stiffness, p0, p1);
InitPointParam (pointDataQ, m_stiffness, q0, q1);
CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]);
CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]);
CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]);
CalculatePointDerivative (3, params, dir1, pointDataQ, &m_jointForce[3]);
CalculatePointDerivative (4, params, dir2, pointDataQ, &m_jointForce[4]);
dgInt32 ret = 5;
if (m_jointAccelFnt) {
dgJointCallbackParam axisParam;
axisParam.m_accel = dgFloat32 (0.0f);
axisParam.m_timestep = params.m_timestep;
axisParam.m_minFriction = DG_MIN_BOUND;
axisParam.m_maxFriction = DG_MAX_BOUND;
if (m_jointAccelFnt (*this, &axisParam)) {
if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) {
params.m_forceBounds[5].m_low = axisParam.m_minFriction;
params.m_forceBounds[5].m_upper = axisParam.m_maxFriction;
params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT;
}
CalculateAngularDerivative (5, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[5]);
// params.m_jointAccel[5] = axisParam.m_accel;
SetMotorAcceleration (5, axisParam.m_accel, params);
ret = 6;
}
}
return dgUnsigned32 (ret);
}
dgQuaternion::dgQuaternion (const dgMatrix& matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dgFloat32 trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
if (trace > dgFloat32(0.0f)) {
trace = dgSqrt (trace + dgFloat32(1.0f));
m_q0 = dgFloat32 (0.5f) * trace;
trace = dgFloat32 (0.5f) / trace;
m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dgFloat32(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dgSqrt (trace);
dgFloat32* const ptr = &m_q1;
ptr[i] = dgFloat32 (0.5f) * trace;
trace = dgFloat32 (0.5f) / trace;
m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#ifdef _DEBUG
dgMatrix tmp (*this, matrix.m_posit);
dgMatrix unitMatrix (tmp * matrix.Inverse());
for (dgInt32 i = 0; i < 4; i ++) {
dgFloat32 err = dgAbsf (unitMatrix[i][i] - dgFloat32(1.0f));
dgAssert (err < dgFloat32 (1.0e-2f));
}
dgFloat32 err = dgAbsf (DotProduct(*this) - dgFloat32(1.0f));
dgAssert (err < dgFloat32(dgEPSILON * 100.0f));
#endif
}
dgInt32 dgCollisionBox::CalculateSignature (dgFloat32 dx, dgFloat32 dy, dgFloat32 dz)
{
dgUnsigned32 buffer[4];
dx = dgAbsf (dx);
dy = dgAbsf (dy);
dz = dgAbsf (dz);
buffer[0] = m_boxCollision;
buffer[1] = Quantize (dx * dgFloat32 (0.5f));
buffer[2] = Quantize (dy * dgFloat32 (0.5f));
buffer[3] = Quantize (dz * dgFloat32 (0.5f));
return Quantize(buffer, sizeof (buffer));
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
if (dgAbsf (x) > dgFloat32 (0.9999f)) {
return dgVector ((x > dgFloat32 (0.0f)) ? m_height : - m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
dgVector sideDir (m_yzMask & dir);
sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt());
return sideDir.Scale4(m_radius) + dir.Scale4 (m_height);
}
dgVector dgCollisionChamferCylinder::SupportVertexSpecial (const dgVector& dir, dgInt32* const vertexIndex) const
{
*vertexIndex = -1;
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
if (dgAbsf (x) > dgFloat32 (0.99995f)) {
return dgVector::m_zero;
}
dgVector sideDir (m_yzMask & dir);
dgAssert ((sideDir % sideDir) > dgFloat32 (0.0f));
return sideDir.CompProduct4(sideDir.InvMagSqrt()).Scale4(m_radius);
}
dgInt32 dgCollisionCone::CalculatePlaneIntersection (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 i;
dgInt32 count;
dgFloat32 y;
dgFloat32 z;
dgFloat32 cosAng;
dgFloat32 sinAng;
dgFloat32 magInv;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
// magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
// cosAng = normal.m_y * magInv;
// sinAng = normal.m_z * magInv;
// dgMatrix matrix (dgGetIdentityMatrix ());
// matrix[1][1] = cosAng;
// matrix[1][2] = sinAng;
// matrix[2][1] = -sinAng;
// matrix[2][2] = cosAng;
// dgVector normal2 (matrix.UnrotateVector (normal));
// dgVector origin2 (matrix.UnrotateVector (origin));
// count = dgCollisionConvex::CalculatePlaneIntersection (normal1, origin1, contactsOut);
// matrix.TransformTriplex (contactsOut, sizeof (dgVector), contactsOut, sizeof (dgVector), count);
magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
cosAng = normal.m_y * magInv;
sinAng = normal.m_z * magInv;
_ASSERTE (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
// dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng,
// normal.m_z * cosAng - normal.m_y * sinAng, dgFloat32 (0.0f));
dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng,
origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f));
count = dgCollisionConvex::CalculatePlaneIntersection (normal1, origin1, contactsOut);
for (i = 0; i < count; i ++) {
y = contactsOut[i].m_y;
z = contactsOut[i].m_z;
contactsOut[i].m_y = y * cosAng - z * sinAng;
contactsOut[i].m_z = z * cosAng + y * sinAng;
}
} else {
count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut);
}
return count;
}
dgVector dgCollisionCone::SupportVertexSpecial(const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert(dgAbsf((dir % dir - dgFloat32(1.0f))) < dgFloat32(1.0e-3f));
if (dir.m_x < dgFloat32(-0.9999f)) {
return dgVector(-(m_height - D_CONE_SKIN_THINCKNESS), dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else if (dir.m_x > dgFloat32(0.9999f)) {
return dgVector(m_height - D_CONE_SKIN_THINCKNESS, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else {
dgVector dir_yz(dir);
dir_yz.m_x = dgFloat32(0.0f);
dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar();
dgAssert(mag2 > dgFloat32(0.0f));
dir_yz = dir_yz.Scale4(dgFloat32(1.0f) / dgSqrt(mag2));
dgVector p0(dir_yz.Scale4(m_radius - D_CONE_SKIN_THINCKNESS));
dgVector p1(dgVector::m_zero);
p0.m_x = -(m_height - D_CONE_SKIN_THINCKNESS);
p1.m_x = m_height - D_CONE_SKIN_THINCKNESS;
dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar();
dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar();
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
}
dgInt32 dgCollisionCone::CalculatePlaneIntersectionSimd (const dgVector& normal, const dgVector& origin, dgVector* const contactsOut) const
{
dgInt32 count;
if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) {
simd_128 normalYZ ((simd_128&) normal & simd_128 (0, -1, -1, 0));
normalYZ = normalYZ * normalYZ.DotProduct(normalYZ).InvSqrt();
dgVector sincos (normalYZ);
dgVector normal1 (normal.m_x, normal.m_y * sincos.m_y + normal.m_z * sincos.m_z, dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector origin1 (origin.m_x, origin.m_y * sincos.m_y + origin.m_z * sincos.m_z,
origin.m_z * sincos.m_y - origin.m_y * sincos.m_z, dgFloat32 (0.0f));
count = dgCollisionConvex::CalculatePlaneIntersectionSimd (normal1, origin1, contactsOut);
for (dgInt32 i = 0; i < count; i ++) {
dgFloat32 y = contactsOut[i].m_y;
dgFloat32 z = contactsOut[i].m_z;
contactsOut[i].m_y = y * sincos.m_y - z * normal.m_z;
contactsOut[i].m_z = z * sincos.m_y + y * normal.m_z;
}
} else {
count = dgCollisionConvex::CalculatePlaneIntersectionSimd (normal, origin, contactsOut);
}
return count;
}
dgVector dgCollisionTaperedCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
dgFloat32 y0 = m_radio0;
dgFloat32 z0 = dgFloat32 (0.0f);
dgFloat32 y1 = m_radio1;
dgFloat32 z1 = dgFloat32 (0.0f);
dgFloat32 mag2 = dir.m_y * dir.m_y + dir.m_z * dir.m_z;
if (mag2 > dgFloat32 (1.0e-12f)) {
mag2 = dgRsqrt(mag2);
y0 = dir.m_y * m_radio0 * mag2;
z0 = dir.m_z * m_radio0 * mag2;
y1 = dir.m_y * m_radio1 * mag2;
z1 = dir.m_z * m_radio1 * mag2;
}
dgVector p0 ( m_height, y0, z0, dgFloat32 (0.0f));
dgVector p1 (-m_height, y1, z1, dgFloat32 (0.0f));
dgFloat32 dist0 = dir % p0;
dgFloat32 dist1 = dir % p1;
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
static dgFloat32 AspectRatio (dgFloat32 x, dgFloat32 y)
{
dgFloat32 tmp;
x = dgAbsf (x);
y = dgAbsf (y);
if (y < x) {
tmp = y;
y = x;
x = tmp;
}
if (y < 1.0e-12) {
y = 1.0e-12f;
}
return x / y;
}
dgMatrix::dgMatrix (const dgQuaternion &rotation, const dgVector &position)
{
hacd::HaF32 x2 = hacd::HaF32 (2.0f) * rotation.m_q1 * rotation.m_q1;
hacd::HaF32 y2 = hacd::HaF32 (2.0f) * rotation.m_q2 * rotation.m_q2;
hacd::HaF32 z2 = hacd::HaF32 (2.0f) * rotation.m_q3 * rotation.m_q3;
#ifdef _DEBUG
hacd::HaF32 w2 = hacd::HaF32 (2.0f) * rotation.m_q0 * rotation.m_q0;
HACD_ASSERT (dgAbsf (w2 + x2 + y2 + z2 - hacd::HaF32(2.0f)) <hacd::HaF32 (1.0e-3f));
#endif
hacd::HaF32 xy = hacd::HaF32 (2.0f) * rotation.m_q1 * rotation.m_q2;
hacd::HaF32 xz = hacd::HaF32 (2.0f) * rotation.m_q1 * rotation.m_q3;
hacd::HaF32 xw = hacd::HaF32 (2.0f) * rotation.m_q1 * rotation.m_q0;
hacd::HaF32 yz = hacd::HaF32 (2.0f) * rotation.m_q2 * rotation.m_q3;
hacd::HaF32 yw = hacd::HaF32 (2.0f) * rotation.m_q2 * rotation.m_q0;
hacd::HaF32 zw = hacd::HaF32 (2.0f) * rotation.m_q3 * rotation.m_q0;
m_front = dgVector (hacd::HaF32(1.0f) - y2 - z2, xy + zw, xz - yw , hacd::HaF32(0.0f));
m_up = dgVector (xy - zw, hacd::HaF32(1.0f) - x2 - z2, yz + xw , hacd::HaF32(0.0f));
m_right = dgVector (xz + yw, yz - xw, hacd::HaF32(1.0f) - x2 - y2 , hacd::HaF32(0.0f));
m_posit.m_x = position.m_x;
m_posit.m_y = position.m_y;
m_posit.m_z = position.m_z;
m_posit.m_w = hacd::HaF32(1.0f);
}
dgVector dgMatrix::CalcPitchYawRoll () const
{
const hacd::HaF32 minSin = hacd::HaF32(0.99995f);
const dgMatrix& matrix = *this;
hacd::HaF32 roll = hacd::HaF32(0.0f);
hacd::HaF32 pitch = hacd::HaF32(0.0f);
hacd::HaF32 yaw = dgAsin (-ClampValue (matrix[0][2], hacd::HaF32(-0.999999f), hacd::HaF32(0.999999f)));
HACD_ASSERT (dgCheckFloat (yaw));
if (matrix[0][2] < minSin) {
if (matrix[0][2] > (-minSin)) {
roll = dgAtan2 (matrix[0][1], matrix[0][0]);
pitch = dgAtan2 (matrix[1][2], matrix[2][2]);
} else {
pitch = dgAtan2 (matrix[1][0], matrix[1][1]);
}
} else {
pitch = -dgAtan2 (matrix[1][0], matrix[1][1]);
}
#ifdef _DEBUG
dgMatrix m (dgPitchMatrix (pitch) * dgYawMatrix(yaw) * dgRollMatrix(roll));
for (hacd::HaI32 i = 0; i < 3; i ++) {
for (hacd::HaI32 j = 0; j < 3; j ++) {
hacd::HaF32 error = dgAbsf (m[i][j] - matrix[i][j]);
HACD_ASSERT (error < 5.0e-2f);
}
}
#endif
return dgVector (pitch, yaw, roll, hacd::HaF32(0.0f));
}
dgVector dgCollisionChamferCylinder::ConvexConicSupporVertex (const dgVector& dir) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
//if (dgAbsf (dir.m_x) > dgFloat32 (0.99995f)) {
if (dgAbsf (x) > dgFloat32 (0.99995f)) {
return dgVector (dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f), dgFloat32 (0.0f));
}
//dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
dgVector sideDir (m_yzMask & dir);
dgAssert ((sideDir % sideDir) > dgFloat32 (0.0f));
//return sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir));
return sideDir.CompProduct4(sideDir.InvMagSqrt()).Scale4(m_radius);
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir) const
{
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
if (dgAbsf (dir.m_x) > dgFloat32 (0.9998f)) {
dgFloat32 x0;
x0 = (dir.m_x >= dgFloat32 (0.0f)) ? m_height : -m_height;
return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f));
}
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
sideDir = sideDir.Scale (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f)));
return sideDir + dir.Scale (m_height);
}
dgVector dgCollisionCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
if (dir.m_x < dgFloat32(-0.9999f)) {
return dgVector(-m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else if (dir.m_x > dgFloat32(0.9999f)) {
return dgVector(m_height, dgFloat32(0.0f), dgFloat32(0.0f), dgFloat32(0.0f));
} else {
dgVector dir_yz (dir);
dir_yz.m_x = dgFloat32 (0.0f);
dgFloat32 mag2 = dir_yz.DotProduct4(dir_yz).GetScalar();
dgAssert (mag2 > dgFloat32 (0.0f));
dir_yz = dir_yz.Scale4 (dgFloat32 (1.0f) / dgSqrt (mag2));
dgVector p0 (dir_yz.Scale4 (m_radio0));
dgVector p1 (dir_yz.Scale4 (m_radio1));
p0.m_x = -m_height;
p1.m_x = m_height;
dgFloat32 dist0 = dir.DotProduct4(p0).GetScalar();
dgFloat32 dist1 = dir.DotProduct4(p1).GetScalar();
if (dist1 >= dist0) {
p0 = p1;
}
return p0;
}
}
dgQuaternion::dgQuaternion (const dgVector &unitAxis, dgFloat32 angle)
{
angle *= dgFloat32 (0.5f);
m_q0 = dgCos (angle);
dgFloat32 sinAng = dgSin (angle);
#ifdef _DEBUG
if (dgAbsf (angle) > dgFloat32(dgEPSILON / 10.0f)) {
dgAssert (dgAbsf (dgFloat32(1.0f) - unitAxis % unitAxis) < dgFloat32(dgEPSILON * 10.0f));
}
#endif
m_q1 = unitAxis.m_x * sinAng;
m_q2 = unitAxis.m_y * sinAng;
m_q3 = unitAxis.m_z * sinAng;
}
dgMatrix::dgMatrix (const dgQuaternion &rotation, const dgVector &position)
{
dgFloat32 x2 = dgFloat32 (2.0f) * rotation.m_q1 * rotation.m_q1;
dgFloat32 y2 = dgFloat32 (2.0f) * rotation.m_q2 * rotation.m_q2;
dgFloat32 z2 = dgFloat32 (2.0f) * rotation.m_q3 * rotation.m_q3;
#ifdef _DEBUG
dgFloat32 w2 = dgFloat32 (2.0f) * rotation.m_q0 * rotation.m_q0;
dgAssert (dgAbsf (w2 + x2 + y2 + z2 - dgFloat32(2.0f)) <dgFloat32 (1.0e-3f));
#endif
dgFloat32 xy = dgFloat32 (2.0f) * rotation.m_q1 * rotation.m_q2;
dgFloat32 xz = dgFloat32 (2.0f) * rotation.m_q1 * rotation.m_q3;
dgFloat32 xw = dgFloat32 (2.0f) * rotation.m_q1 * rotation.m_q0;
dgFloat32 yz = dgFloat32 (2.0f) * rotation.m_q2 * rotation.m_q3;
dgFloat32 yw = dgFloat32 (2.0f) * rotation.m_q2 * rotation.m_q0;
dgFloat32 zw = dgFloat32 (2.0f) * rotation.m_q3 * rotation.m_q0;
m_front = dgVector (dgFloat32(1.0f) - y2 - z2, xy + zw, xz - yw , dgFloat32(0.0f));
m_up = dgVector (xy - zw, dgFloat32(1.0f) - x2 - z2, yz + xw , dgFloat32(0.0f));
m_right = dgVector (xz + yw, yz - xw, dgFloat32(1.0f) - x2 - y2 , dgFloat32(0.0f));
m_posit.m_x = position.m_x;
m_posit.m_y = position.m_y;
m_posit.m_z = position.m_z;
m_posit.m_w = dgFloat32(1.0f);
}
static hacd::HaF32 AspectRatio (hacd::HaF32 x, hacd::HaF32 y)
{
hacd::HaF32 tmp;
x = dgAbsf (x);
y = dgAbsf (y);
if (y < x) {
tmp = y;
y = x;
x = tmp;
}
if (y < 1.0e-12) {
y = 1.0e-12f;
}
return x / y;
}
void dgCollisionDeformableMesh::SetSkinThickness (dgFloat32 skinThickness)
{
m_skinThickness = dgAbsf (skinThickness);
if (m_skinThickness < DG_DEFORMABLE_DEFAULT_SKIN_THICKNESS) {
m_skinThickness = DG_DEFORMABLE_DEFAULT_SKIN_THICKNESS;
}
SetCollisionBBox (m_rootNode->m_minBox, m_rootNode->m_maxBox);
}
dgVector dgCollisionChamferCylinder::SupportVertex (const dgVector& dir, dgInt32* const vertexIndex) const
{
dgAssert (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 x = dir.GetScalar();
if (dgAbsf (x) > dgFloat32 (0.9999f)) {
dgFloat32 x0 = (x >= dgFloat32 (0.0f)) ? m_height : -m_height;
return dgVector (x0, dgFloat32 (0.0f), m_radius, dgFloat32 (0.0f));
}
// dgVector sideDir (dgFloat32 (0.0f), dir.m_y, dir.m_z, dgFloat32 (0.0f));
dgVector sideDir (m_yzMask & dir);
// sideDir = sideDir.Scale3 (m_radius * dgRsqrt (sideDir % sideDir + dgFloat32 (1.0e-18f)));
sideDir = sideDir.CompProduct4(sideDir.InvMagSqrt());
// return sideDir + dir.Scale3 (m_height);
return sideDir.Scale4(m_radius) + dir.Scale4 (m_height);
}
dgVector dgCollisionTaperedCapsule::ConvexConicSupporVertex (const dgVector& point, const dgVector& dir) const
{
dgAssert (dgAbsf (dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgVector p (SupportVertex(dir, NULL));
dgVector n (dir.m_x, dgSqrt (dir.m_y * dir.m_y + dir.m_z * dir.m_z), dgFloat32 (0.0), dgFloat32 (0.0f));
dgAssert (dgAbsf (n % n - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
dgFloat32 project = m_sideNormal % n;
if (project > dgFloat32 (0.9998f)) {
dgFloat32 t = dgFloat32 (0.5f) * (point.m_x + m_height) / m_height;
dgFloat32 r = m_radio1 + (m_radio0 - m_radio1) * t;
p = dir.Scale3 (r);
p.m_x += point.m_x;
}
return p;
}
dgInt32 dgCollisionSphere::CalculateSignature (dgFloat32 radius)
{
dgUnsigned32 buffer[2];
radius = dgAbsf (radius);
buffer[0] = m_sphereCollision;
buffer[1] = Quantize (radius);
return Quantize(buffer, sizeof (buffer));
}
dgVector dgCollisionCone::SupportVertex (const dgVector& dir) const
{
/*
dgFloat32 y0;
dgFloat32 z0;
dgFloat32 y1;
dgFloat32 z1;
dgFloat32 dist0;
dgFloat32 dist1;
dgFloat32 tetha;
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
if (dir.m_x > m_sinAngle) {
return dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
tetha = m_tethaStep * dgFloor (dgAtan2 (dir.m_y, dir.m_z) * m_tethaStepInv);
dgSinCos (tetha, y0, z0);
y0 *= m_radius;
z0 *= m_radius;
y1 = y0 * m_delCosTetha + z0 * m_delSinTetha;
z1 = z0 * m_delCosTetha - y0 * m_delSinTetha;
dist0 = dir.m_y * y0 + dir.m_z * z0;
dist1 = dir.m_y * y1 + dir.m_z * z1;
if (dist1 > dist0) {
y0 = y1;
z0 = z1;
}
return dgVector (-m_height, y0, z0, dgFloat32 (0.0f));
*/
dgFloat32 y0;
dgFloat32 z0;
dgFloat32 mag2;
_ASSERTE (dgAbsf(dir % dir - dgFloat32 (1.0f)) < dgFloat32 (1.0e-3f));
if (dir.m_x > m_sinAngle) {
return dgVector (m_height, dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
}
y0 = m_radius;
z0 = dgFloat32 (0.0f);
mag2 = dir.m_y * dir.m_y + dir.m_z * dir.m_z;
if (mag2 > dgFloat32 (1.0e-12f)) {
mag2 = dgRsqrt(mag2);
y0 = dir.m_y * m_radius * mag2;
z0 = dir.m_z * m_radius * mag2;
}
return dgVector (-m_height, y0, z0, dgFloat32 (0.0f));
}
