static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
dVector3 p1, dVector3 p2, dVector3 p3)
{
int k;
dVector3 u1,u2,n,tmp;
for (k=0; k<3; k++) u1[k] = p3[k]-p1[k];
for (k=0; k<3; k++) u2[k] = p2[k]-p1[k];
dReal d1 = dSqrt(dCalcVectorDot3(u1,u1));
dReal d2 = dSqrt(dCalcVectorDot3(u2,u2));
dNormalize3 (u1);
dNormalize3 (u2);
if (dFabs(dCalcVectorDot3(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2");
dCalcVectorCross3(n,u1,u2);
for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k];
dReal d = -dCalcVectorDot3(n,p1);
if (dFabs(dCalcVectorDot3(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1");
if (dFabs(dCalcVectorDot3(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2");
if (dFabs(dCalcVectorDot3(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3");
dReal alpha = -(d+dCalcVectorDot3(n,v1))/dCalcVectorDot3(n,tmp);
for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]);
if (dFabs(dCalcVectorDot3(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp");
if (alpha < 0) return 0;
if (alpha > 1) return 0;
for (k=0; k<3; k++) tmp[k] -= p1[k];
dReal a1 = dCalcVectorDot3(u1,tmp);
dReal a2 = dCalcVectorDot3(u2,tmp);
if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0;
return 1;
}
static int MakeRandomGuassianPointCloud (NewtonMesh* const mesh, dVector* const points, int count)
{
dVector size(0.0f);
dMatrix matrix(dGetIdentityMatrix());
NewtonMeshCalculateOOBB(mesh, &matrix[0][0], &size.m_x, &size.m_y, &size.m_z);
dVector minBox (matrix.m_posit - matrix[0].Scale (size.m_x) - matrix[1].Scale (size.m_y) - matrix[2].Scale (size.m_z));
dVector maxBox (matrix.m_posit + matrix[0].Scale (size.m_x) + matrix[1].Scale (size.m_y) + matrix[2].Scale (size.m_z));
size = (maxBox - minBox).Scale (0.5f);
dVector origin = (maxBox + minBox).Scale (0.5f);
dFloat biasExp = 10.0f;
dFloat r = dSqrt (size.DotProduct3(size));
r = powf(r, 1.0f/biasExp);
for (int i = 0; i < count; i++) {
dVector& p = points[i];
bool test;
do {
p = dVector (2.0f * RandomVariable(r), 2.0f * RandomVariable(r), 2.0f * RandomVariable(r), 0.0f);
dFloat len = dSqrt (p.DotProduct3(p));
dFloat scale = powf(len, biasExp) / len;
p = p.Scale (scale) + origin;
test = (p.m_x > minBox.m_x) && (p.m_x < maxBox.m_x) && (p.m_y > minBox.m_y) && (p.m_y < maxBox.m_y) && (p.m_z > minBox.m_z) && (p.m_z < maxBox.m_z);
} while (!test);
}
return count;
}
void SubmitConstraints(dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix(matrix0, matrix1);
dVector p0(matrix0.m_posit);
dVector p1(matrix1.m_posit);
dVector dir(p1 - p0);
dFloat mag2 = dir % dir;
dir = dir.Scale(1.0f / dSqrt(mag2));
dMatrix matrix(dGrammSchmidt(dir));
dFloat x = dSqrt(mag2) - m_distance;
dVector com0;
dVector com1;
dVector veloc0;
dVector veloc1;
dMatrix body0Matrix;
dMatrix body1Matrix;
NewtonBody* const body0 = GetBody0();
NewtonBody* const body1 = GetBody1();
NewtonBodyGetCentreOfMass(body0, &com0[0]);
NewtonBodyGetMatrix(body0, &body0Matrix[0][0]);
NewtonBodyGetPointVelocity(body0, &p0[0], &veloc0[0]);
NewtonBodyGetCentreOfMass(body1, &com1[0]);
NewtonBodyGetMatrix(body1, &body1Matrix[0][0]);
NewtonBodyGetPointVelocity(body1, &p1[0], &veloc1[0]);
dFloat v((veloc0 - veloc1) % dir);
dFloat a = (x - v * timestep) / (timestep * timestep);
dVector r0((p0 - body0Matrix.TransformVector(com0)) * matrix.m_front);
dVector r1((p1 - body1Matrix.TransformVector(com1)) * matrix.m_front);
dFloat jacobian0[6];
dFloat jacobian1[6];
jacobian0[0] = matrix[0][0];
jacobian0[1] = matrix[0][1];
jacobian0[2] = matrix[0][2];
jacobian0[3] = r0[0];
jacobian0[4] = r0[1];
jacobian0[5] = r0[2];
jacobian1[0] = -matrix[0][0];
jacobian1[1] = -matrix[0][1];
jacobian1[2] = -matrix[0][2];
jacobian1[3] = -r1[0];
jacobian1[4] = -r1[1];
jacobian1[5] = -r1[2];
NewtonUserJointAddGeneralRow(m_joint, jacobian0, jacobian1);
NewtonUserJointSetRowAcceleration(m_joint, a);
}
void BodyCutForce(dBodyID body,float l_limit,float w_limit)
{
const dReal wa_limit=w_limit/fixed_step;
const dReal* force= dBodyGetForce(body);
dReal force_mag=dSqrt(dDOT(force,force));
//body mass
dMass m;
dBodyGetMass(body,&m);
dReal force_limit =l_limit/fixed_step*m.mass;
if(force_mag>force_limit)
{
dBodySetForce(body,
force[0]/force_mag*force_limit,
force[1]/force_mag*force_limit,
force[2]/force_mag*force_limit
);
}
const dReal* torque=dBodyGetTorque(body);
dReal torque_mag=dSqrt(dDOT(torque,torque));
if(torque_mag<0.001f) return;
dMatrix3 tmp,invI,I;
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,m.I,body->R);
dMULTIPLY0_333 (I,body->R,tmp);
// compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp,body->invI,body->R);
dMULTIPLY0_333 (invI,body->R,tmp);
//angular accel
dVector3 wa;
dMULTIPLY0_331(wa,invI,torque);
dReal wa_mag=dSqrt(dDOT(wa,wa));
if(wa_mag>wa_limit)
{
//scale w
for(int i=0;i<3;++i)wa[i]*=wa_limit/wa_mag;
dVector3 new_torqu;
dMULTIPLY0_331(new_torqu,I,wa);
dBodySetTorque
(
body,
new_torqu[0],
new_torqu[1],
new_torqu[2]
);
}
}
dQuaternion::dQuaternion (const dMatrix &matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
dAssert (((matrix[0] * matrix[1]) % matrix[2]) > 0.0f);
if (trace > dFloat(0.0f)) {
trace = dSqrt (trace + dFloat(1.0f));
m_q0 = dFloat (0.5f) * trace;
trace = dFloat (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 = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dSqrt (trace);
dFloat* const ptr = &m_q1;
ptr[i] = dFloat (0.5f) * trace;
trace = dFloat (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;
}
#if _DEBUG
dMatrix tmp (*this, matrix.m_posit);
dMatrix unitMatrix (tmp * matrix.Inverse());
for (int i = 0; i < 4; i ++) {
dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
dAssert (err < dFloat (1.0e-3f));
}
dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
dAssert (err < dFloat(1.0e-3f));
#endif
}
//void dMatrix::PolarDecomposition (dMatrix& orthogonal, dMatrix& symetric) const
void dMatrix::PolarDecomposition (dMatrix & transformMatrix, dVector & scale, dMatrix & stretchAxis, const dMatrix & initialStretchAxis) const
{
// a polar decomposition decompose matrix A = O * S
// where S = sqrt (transpose (L) * L)
// calculate transpose (L) * L
dMatrix LL ((*this) * Transpose());
// check is this si a pure uniformScale * rotation * translation
dFloat det2 = (LL[0][0] + LL[1][1] + LL[2][2]) * (1.0f / 3.0f);
dFloat invdet2 = 1.0f / det2;
dMatrix pureRotation (LL);
pureRotation[0] = pureRotation[0].Scale (invdet2);
pureRotation[1] = pureRotation[1].Scale (invdet2);
pureRotation[2] = pureRotation[2].Scale (invdet2);
//dFloat soureSign = ((*this)[0] * (*this)[1]) % (*this)[2];
dFloat sign = ((((*this)[0] * (*this)[1]) % (*this)[2]) > 0.0f) ? 1.0f : -1.0f;
dFloat det = (pureRotation[0] * pureRotation[1]) % pureRotation[2];
if (dAbs (det - 1.0f) < 1.e-5f)
{
// this is a pure scale * rotation * translation
det = sign * dSqrt (det2);
scale[0] = det;
scale[1] = det;
scale[2] = det;
scale[3] = 1.0f;
det = 1.0f / det;
transformMatrix.m_front = m_front.Scale (det);
transformMatrix.m_up = m_up.Scale (det);
transformMatrix.m_right = m_right.Scale (det);
transformMatrix[0][3] = 0.0f;
transformMatrix[1][3] = 0.0f;
transformMatrix[2][3] = 0.0f;
transformMatrix.m_posit = m_posit;
stretchAxis = dGetIdentityMatrix();
}
else
{
stretchAxis = LL.JacobiDiagonalization (scale, initialStretchAxis);
// I need to deal with buy seeing of some of the Scale are duplicated
// do this later (maybe by a given rotation around the non uniform axis but I do not know if it will work)
// for now just us the matrix
scale[0] = sign * dSqrt (scale[0]);
scale[1] = sign * dSqrt (scale[1]);
scale[2] = sign * dSqrt (scale[2]);
scale[3] = 1.0f;
dMatrix scaledAxis;
scaledAxis[0] = stretchAxis[0].Scale (1.0f / scale[0]);
scaledAxis[1] = stretchAxis[1].Scale (1.0f / scale[1]);
scaledAxis[2] = stretchAxis[2].Scale (1.0f / scale[2]);
scaledAxis[3] = stretchAxis[3];
dMatrix symetricInv (stretchAxis.Transpose() * scaledAxis);
transformMatrix = symetricInv * (*this);
transformMatrix.m_posit = m_posit;
}
}
void dQfromR (dQuaternion q, const dMatrix3 R)
{
dAASSERT (q && R);
dReal tr(0), s(0);
tr = _R(0,0) + _R(1,1) + _R(2,2);
if (tr >= 0)
{
s = dSqrt (tr + 1);
q[0] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[1] = (_R(2,1) - _R(1,2)) * s;
q[2] = (_R(0,2) - _R(2,0)) * s;
q[3] = (_R(1,0) - _R(0,1)) * s;
}
else {
// find the largest diagonal element and jump to the appropriate case
if (_R(1,1) > _R(0,0)) {
if (_R(2,2) > _R(1,1)) goto case_2;
goto case_1;
}
if (_R(2,2) > _R(0,0)) goto case_2;
goto case_0;
case_0:
s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + 1);
q[1] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[2] = (_R(0,1) + _R(1,0)) * s;
q[3] = (_R(2,0) + _R(0,2)) * s;
q[0] = (_R(2,1) - _R(1,2)) * s;
return;
case_1:
s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + 1);
q[2] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[3] = (_R(1,2) + _R(2,1)) * s;
q[1] = (_R(0,1) + _R(1,0)) * s;
q[0] = (_R(0,2) - _R(2,0)) * s;
return;
case_2:
s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + 1);
q[3] = REAL(0.5) * s;
s = REAL(0.5) * dRecip(s);
q[1] = (_R(2,0) + _R(0,2)) * s;
q[2] = (_R(1,2) + _R(2,1)) * s;
q[0] = (_R(1,0) - _R(0,1)) * s;
return;
}
}
EXPORT_C void dQfromR (dQuaternion q, const dMatrix3 R)
{
dReal tr,s;
tr = _R(0,0) + _R(1,1) + _R(2,2);
if (tr >= 0) {
s = dSqrt (tr + REAL(1.0));
q[0] = dMUL(REAL(0.5),s);
s = dMUL(REAL(0.5),dRecip(s));
q[1] = dMUL((_R(2,1) - _R(1,2)),s);
q[2] = dMUL((_R(0,2) - _R(2,0)),s);
q[3] = dMUL((_R(1,0) - _R(0,1)),s);
}
else {
// find the largest diagonal element and jump to the appropriate case
if (_R(1,1) > _R(0,0)) {
if (_R(2,2) > _R(1,1)) goto case_2;
goto case_1;
}
if (_R(2,2) > _R(0,0)) goto case_2;
goto case_0;
case_0:
s = dSqrt((_R(0,0) - (_R(1,1) + _R(2,2))) + REAL(1.0));
q[1] = dMUL(REAL(0.5),s);
s = dMUL(REAL(0.5),dRecip(s));
q[2] = dMUL((_R(0,1) + _R(1,0)),s);
q[3] = dMUL((_R(2,0) + _R(0,2)),s);
q[0] = dMUL((_R(2,1) - _R(1,2)),s);
return;
case_1:
s = dSqrt((_R(1,1) - (_R(2,2) + _R(0,0))) + REAL(1.0));
q[2] = dMUL(REAL(0.5),s);
s = dMUL(REAL(0.5),dRecip(s));
q[3] = dMUL((_R(1,2) + _R(2,1)),s);
q[1] = dMUL((_R(0,1) + _R(1,0)),s);
q[0] = dMUL((_R(0,2) - _R(2,0)),s);
return;
case_2:
s = dSqrt((_R(2,2) - (_R(0,0) + _R(1,1))) + REAL(1.0));
q[3] = dMUL(REAL(0.5),s);
s = dMUL(REAL(0.5),dRecip(s));
q[1] = dMUL((_R(2,0) + _R(0,2)),s);
q[2] = dMUL((_R(1,2) + _R(2,1)),s);
q[0] = dMUL((_R(1,0) - _R(0,1)),s);
return;
}
}
static void MagneticField (const NewtonJoint* contactJoint, dFloat timestep, int threadIndex)
{
dFloat magnetStregnth;
const NewtonBody* body0;
const NewtonBody* body1;
const NewtonBody* magneticField;
const NewtonBody* magneticPiece;
body0 = NewtonJointGetBody0 (contactJoint);
body1 = NewtonJointGetBody1 (contactJoint);
// get the magnetic field body
magneticPiece = body0;
magneticField = body1;
if (NewtonCollisionIsTriggerVolume (NewtonBodyGetCollision(body0))) {
magneticPiece = body1;
magneticField = body0;
}
_ASSERTE (NewtonCollisionIsTriggerVolume (NewtonBodyGetCollision(magneticField)));
// calculate the magnetic force field
dMatrix center;
dMatrix location;
NewtonBodyGetMatrix (magneticField, ¢er[0][0]);
NewtonBodyGetMatrix (magneticPiece, &location[0][0]);
Magnet* magnet;
magnet = (Magnet*)NewtonBodyGetUserData(magneticField);
magnetStregnth = magnet->m_magnetStregnth;
// calculate the magnetic force;
dFloat den;
dVector force (center.m_posit - location.m_posit);
den = force % force;
den = magnetStregnth / (den * dSqrt (den) + 0.1f);
force = force.Scale (den);
// because we are modifiing one of the bodies membber in the call back, there uis a chace that
// another materail can be operations on the same object at the same time of aother thread
// therfore we need to make the assigmnet in a critical section.
NewtonWorldCriticalSectionLock (NewtonBodyGetWorld (magneticPiece));
// add the magner force
NewtonBodyAddForce (magneticPiece, &force[0]);
force = force.Scale (-1.0f);
NewtonBodyAddForce (magnet->m_magneticCore, &force[0]);
// also if the body is sleeping fore it to wake up for this frame
NewtonBodySetFreezeState (magneticPiece, 0);
NewtonBodySetFreezeState (magnet->m_magneticCore, 0);
// unlock the critical section
NewtonWorldCriticalSectionUnlock (NewtonBodyGetWorld (magneticPiece));
}
virtual void SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix;
dVector com;
const dFloat speed = 3.0f;
NewtonBody* const body = GetBody0();
NewtonBodyGetCentreOfMass(body, &com[0]);
NewtonBodyGetMatrix(body, &matrix[0][0]);
com = matrix.TransformVector(com);
switch (m_state)
{
case m_stop:
{
SetTargetPosit (com);
break;
}
case m_driving:
{
dVector veloc (m_target - com);
veloc = veloc.Scale (speed / dSqrt (veloc % veloc));
dVector target = com + veloc.Scale(timestep);
SetTargetPosit (target);
break;
}
default:;
dAssert (0);
}
CustomKinematicController::SubmitConstraints (timestep, threadIndex);
}
int dFactorCholesky (dReal *A, int n)
{
int i,j,k,nskip;
dReal sum,*a,*b,*aa,*bb,*cc,*recip;
dAASSERT (n > 0 && A);
nskip = dPAD (n);
recip = (dReal*) ALLOCA (n * sizeof(dReal));
aa = A;
for (i=0; i<n; i++) {
bb = A;
cc = A + i*nskip;
for (j=0; j<i; j++) {
sum = *cc;
a = aa;
b = bb;
for (k=j; k; k--) sum -= (*(a++))*(*(b++));
*cc = sum * recip[j];
bb += nskip;
cc++;
}
sum = *cc;
a = aa;
for (k=i; k; k--, a++) sum -= (*a)*(*a);
if (sum <= REAL(0.0)) return 0;
*cc = dSqrt(sum);
recip[i] = dRecip (*cc);
aa += nskip;
}
return 1;
}
void CPHCapture::CapturedUpdate()
{
m_island.Unmerge();
if(m_character->CPHObject::is_active())
{
m_taget_element->Enable();
}
if(!m_taget_element->isActive()||dDOT(m_joint_feedback.f2,m_joint_feedback.f2)>m_capture_force*m_capture_force)
{
Release();
return;
}
float mag=dSqrt(dDOT(m_joint_feedback.f1,m_joint_feedback.f1));
//m_back_force=m_back_force*0.999f+ ((mag<m_capture_force/5.f) ? mag : (m_capture_force/5.f))*0.001f;
//
if(b_character_feedback&&mag>m_capture_force/2.2f)
{
float f=mag/(m_capture_force/15.f);
m_character->ApplyForce(m_joint_feedback.f1[0]/f,m_joint_feedback.f1[1]/f,m_joint_feedback.f1[2]/f);
}
Fvector capture_bone_position;
//CObject* object=smart_cast<CObject*>(m_character->PhysicsRefObject());
capture_bone_position.set(m_capture_bone->mTransform.c);
m_character->PhysicsRefObject()->ObjectXFORM().transform_tiny(capture_bone_position);
dBodySetPosition(m_body,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z);
}
void dCustomUpVector::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
dMatrix matrix1;
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (matrix0, matrix1);
// if the body ha rotated by some amount, the there will be a plane of rotation
dVector lateralDir (matrix0.m_front.CrossProduct(matrix1.m_front));
dFloat mag = lateralDir.DotProduct3(lateralDir);
if (mag > 1.0e-6f) {
// if the side vector is not zero, it means the body has rotated
mag = dSqrt (mag);
lateralDir = lateralDir.Scale (1.0f / mag);
dFloat angle = dAsin (mag);
// add an angular constraint to correct the error angle
NewtonUserJointAddAngularRow (m_joint, angle, &lateralDir[0]);
// in theory only one correction is needed, but this produces instability as the body may move sideway.
// a lateral correction prevent this from happening.
dVector frontDir (lateralDir.CrossProduct(matrix1.m_front));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &frontDir[0]);
} else {
// if the angle error is very small then two angular correction along the plane axis do the trick
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
}
}
// rolling friction works as follow: the idealization of the contact of a spherical object
// with a another surface is a point that pass by the center of the sphere.
// in most cases this is enough to model the collision but in insufficient for modeling
// the rolling friction. In reality contact with the sphere with the other surface is not
// a point but a contact patch. A contact patch has the property the it generates a fix
// constant rolling torque that opposes the movement of the sphere.
// we can model this torque by adding a clamped torque aligned to the instantaneously axis
// of rotation of the ball. and with a magnitude of the stopping angular acceleration.
void CustomDryRollingFriction::SubmitConstrainst (dFloat timestep, int threadIndex)
{
dVector omega;
dFloat omegaMag;
dFloat torqueFriction;
// get the omega vector
NewtonBodyGetOmega(m_body0, &omega[0]);
omegaMag = dSqrt (omega % omega);
if (omegaMag > 0.1f) {
// tell newton to used this the friction of the omega vector to apply the rolling friction
dVector pin (omega.Scale (1.0f / omegaMag));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &pin[0]);
// calculate the acceleration to stop the ball in one time step
NewtonUserJointSetRowAcceleration (m_joint, -omegaMag / timestep);
// set the friction limit proportional the sphere Inertia
torqueFriction = m_frictionTorque * m_frictionCoef;
NewtonUserJointSetRowMinimumFriction (m_joint, -torqueFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, torqueFriction);
} else {
// when omega is too low sheath a little bit and damp the omega directly
omega = omega.Scale (0.2f);
NewtonBodySetOmega(m_body0, &omega[0]);
}
}
void DebugShowGeometryCollision (void* userData, int vertexCount, const dFloat* const faceVertec, int id)
{
//DEBUG_DRAW_MODE mode = (DEBUG_DRAW_MODE) ((int)userData); //NOTE error: cast from ‘void*’ to ‘int’ loses precision
DEBUG_DRAW_MODE mode = (DEBUG_DRAW_MODE) ((intptr_t)userData);
if (mode == m_lines) {
int index = vertexCount - 1;
dVector p0 (faceVertec[index * 3 + 0], faceVertec[index * 3 + 1], faceVertec[index * 3 + 2]);
for (int i = 0; i < vertexCount; i ++) {
dVector p1 (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
p0 = p1;
}
} else {
dVector p0 (faceVertec[0 * 3 + 0], faceVertec[0 * 3 + 1], faceVertec[0 * 3 + 2]);
dVector p1 (faceVertec[1 * 3 + 0], faceVertec[1 * 3 + 1], faceVertec[1 * 3 + 2]);
dVector p2 (faceVertec[2 * 3 + 0], faceVertec[2 * 3 + 1], faceVertec[2 * 3 + 2]);
dVector normal ((p1 - p0) * (p2 - p0));
normal = normal.Scale (1.0f / dSqrt (normal % normal));
glNormal3f(normal.m_x, normal.m_y, normal.m_z);
for (int i = 2; i < vertexCount; i ++) {
p2 = dVector (faceVertec[i * 3 + 0], faceVertec[i * 3 + 1], faceVertec[i * 3 + 2]);
glVertex3f (p0.m_x, p0.m_y, p0.m_z);
glVertex3f (p1.m_x, p1.m_y, p1.m_z);
glVertex3f (p2.m_x, p2.m_y, p2.m_z);
p1 = p2;
}
}
}
IC bool dcTriListCollider::FragmentonSphereTest(const dReal* center, const dReal radius,
const dReal* pt1, const dReal* pt2,dReal* norm,dReal& depth)
{
dVector3 V={pt2[0]-pt1[0],pt2[1]-pt1[1],pt2[2]-pt1[2]};
dVector3 L={pt1[0]-center[0],pt1[1]-center[1],pt1[2]-center[2]};
dReal sq_mag_V=dDOT(V,V);
dReal dot_L_V=dDOT(L,V);
dReal t=-dot_L_V/sq_mag_V;//t
if(t<0.f||t>1.f)return false;
dVector3 Pc={pt1[0]+t*V[0],pt1[1]+t*V[1],pt1[2]+t*V[2]};
dVector3 Dc={center[0]-Pc[0],center[1]-Pc[1],center[2]-Pc[2]};
dReal sq_mag_Dc=dDOT(Dc,Dc);
if(sq_mag_Dc>radius*radius)return false;
//dReal dot_V_Pc=dDOT(V,Pc);
//if(dDOT(V,pt1)>dot_V_Pc||dDOT(V,pt2)<dot_V_Pc) return false;
dReal mag=dSqrt(sq_mag_Dc);
depth=radius-mag;
if(mag>0.f)
{
norm[0]=Dc[0]/mag;
norm[1]=Dc[1]/mag;
norm[2]=Dc[2]/mag;
}
else
{
norm[0]=0;
norm[1]=1;
norm[2]=0;
}
return true;
}
dQuaternion dQuaternion::Slerp (const dQuaternion &QB, dFloat t) const
{
dFloat dot;
dFloat ang;
dFloat Sclp;
dFloat Sclq;
dFloat den;
dFloat sinAng;
dQuaternion Q;
dot = DotProduct (QB);
_ASSERTE (dot >= 0.0f);
if ((dot + dFloat(1.0f)) > dFloat(1.0e-5f)) {
if (dot < dFloat(0.995f)) {
ang = dAcos (dot);
sinAng = dSin (ang);
den = dFloat(1.0f) / sinAng;
Sclp = dSin ((dFloat(1.0f) - t ) * ang) * den;
Sclq = dSin (t * ang) * den;
} else {
Sclp = dFloat(1.0f) - t;
Sclq = t;
}
Q.m_q0 = m_q0 * Sclp + QB.m_q0 * Sclq;
Q.m_q1 = m_q1 * Sclp + QB.m_q1 * Sclq;
Q.m_q2 = m_q2 * Sclp + QB.m_q2 * Sclq;
Q.m_q3 = m_q3 * Sclp + QB.m_q3 * Sclq;
} else {
Q.m_q0 = m_q3;
Q.m_q1 = -m_q2;
Q.m_q2 = m_q1;
Q.m_q3 = m_q0;
Sclp = dSin ((dFloat(1.0f) - t) * dFloat (3.141592f *0.5f));
Sclq = dSin (t * dFloat (3.141592f * 0.5f));
Q.m_q0 = m_q0 * Sclp + Q.m_q0 * Sclq;
Q.m_q1 = m_q1 * Sclp + Q.m_q1 * Sclq;
Q.m_q2 = m_q2 * Sclp + Q.m_q2 * Sclq;
Q.m_q3 = m_q3 * Sclp + Q.m_q3 * Sclq;
}
dot = Q.DotProduct (Q);
if ((dot) < (1.0f - 1.0e-4f) ) {
dot = dFloat(1.0f) / dSqrt (dot);
//dot = dgRsqrt (dot);
Q.m_q0 *= dot;
Q.m_q1 *= dot;
Q.m_q2 *= dot;
Q.m_q3 *= dot;
}
return Q;
}
NewtonMesh* DemoMesh::CreateNewtonMesh(NewtonWorld* const world, const dMatrix& meshMatrix)
{
NewtonMesh* const mesh = NewtonMeshCreate(world);
NewtonMeshBeginBuild (mesh);
dMatrix rotation ((meshMatrix.Inverse4x4()).Transpose4X4());
for (dListNode* node = GetFirst(); node; node = node->GetNext()) {
DemoSubMesh& segment = node->GetInfo();
for (int i = 0; i < segment.m_indexCount; i += 3) {
NewtonMeshBeginFace(mesh);
for (int j = 0; j < 3; j ++) {
int index = segment.m_indexes[i + j];
dVector p (meshMatrix.TransformVector(dVector (m_vertex[index * 3 + 0], m_vertex[index * 3 + 1], m_vertex[index * 3 + 2], 0.0f)));
dVector n (rotation.RotateVector(dVector (m_normal[index * 3 + 0], m_normal[index * 3 + 1], m_normal[index * 3 + 2], 0.0f)));
dAssert ((n.DotProduct3(n)) > 0.0f);
n = n.Scale (1.0f / dSqrt (n.DotProduct3(n)));
NewtonMeshAddPoint(mesh, p.m_x, p.m_y, p.m_z);
NewtonMeshAddNormal(mesh, n.m_x, n.m_y, n.m_z);
NewtonMeshAddMaterial(mesh, segment.m_textureHandle);
NewtonMeshAddUV0(mesh, m_uv[index * 2 + 0], m_uv[index * 2 + 1]);
}
NewtonMeshEndFace(mesh);
// NewtonMeshAddFace(mesh, 3, &point[0][0], sizeof (point) / 3, segment.m_textureHandle);
}
}
NewtonMeshEndBuild(mesh);
return mesh;
}
int CreateCone (dVector* const points, dVector* const normals, int segments, dFloat radius, dFloat height, int maxPoints)
{
dMatrix rotation (dPitchMatrix((-360.0f / segments) * 3.141592f/180.0f));
dVector p0 (0.0, 0.0f, 0.0f, 0.0f);
dVector p1 (0.0, radius, 0.0f, 0.0f);
dVector q1 (height, 0.0f, 0.0f, 0.0f);
dVector n (radius, height, 0.0f, 0.0f);
n = n.Scale (1.0f / dSqrt (n.DotProduct3(n)));
int count = 0;
for (int i = 0; (i < segments) && (count < maxPoints); i ++) {
dVector p2 (rotation.RotateVector(p1));
normals[count] = dVector (-1.0f, 0.0f, 0.0f, 0.0f);
points[count * 3 + 0] = p0;
points[count * 3 + 1] = p1;
points[count * 3 + 2] = p2;
count ++;
normals[count] = dVector (n.m_x, n.m_y, n.m_z, 0.0f);
points[count * 3 + 0] = p1;
points[count * 3 + 1] = q1;
points[count * 3 + 2] = p2;
count ++;
n = rotation.RotateVector(n);
p1 = p2;
}
return count;
}
static dErr JakoSIAVelocity(VHTCase scase,dReal b,dReal h,dReal dh[2],dReal z,dScalar u[3])
{
struct VHTRheology *rheo = &scase->rheo;
const dReal
p = rheo->pe,
n = 1 ? 1 : 1./(p-1),
B = rheo->B0 * pow(2*rheo->gamma0,(n-1)/(2*n)), // reduce to Stress = B |Du|^{1/n}
A = pow(B,-n);
const dScalar
slope = dSqrt(dSqr(dh[0]) + dSqr(dh[1])),
sliding = 0,
hmz = z > h ? 0 : (z < b ? h-b : h-z),
// The strain rate is: Du = A tau^n
// where: tau = rho*grav*(h-z)*dh
int_bz = -1. / (n+1) * (pow(hmz,n+1) - pow(h-b,n+1)), // \int_b^z (h-z)^n
//bstress = rheo->rhoi * dAbs(rheo->gravity[2]) * (h-b) * slope, // diagnostic
//rstress = dUnitNonDimensionalizeSI(scase->utable.Pressure,1e5),
siaspeed = sliding + A * pow(rheo->rhoi * dAbs(rheo->gravity[2]) * slope, n) * int_bz, // Integrate strain rate from the bottom
speed = siaspeed * (5 + 0 * (1 + tanh((z-b)/100))/2);
u[0] = -speed / slope * dh[0];
u[1] = -speed / slope * dh[1];
u[2] = 0; // Would need another derivative to evaluate this and it should not be a big deal for this stationary computation
//printf("u0 %g u1 %g rho %g grav %g stress %g 1bar %g dh[2] %g %g A %g B %g;\n",u[0],u[1],rheo->rhoi,rheo->gravity[2],bstress,rstress,dh[0],dh[1],A,B);
return 0;
}
void dBezierSpline::CreateCubicKnotVector(int count, const dBigVector* const points)
{
dFloat64 d = 0.0f;
dAssert (count >= 2);
dFloat64 u[D_BEZIER_LOCAL_BUFFER_SIZE];
u[0] = 0.0f;
for (int i = 1; i < count; i ++) {
dBigVector step (points[i] - points[i - 1]);
dFloat64 len = dSqrt (step.DotProduct3(step));
// u[i] = len;
u[i] = dSqrt (len);
d += u[i];
}
for (int i = 1; i < count; i ++) {
u[i] = u[i-1] + u[i] / d;
}
u[0] = 0.0f;
u[count - 1] = 1.0f;
m_degree = 3;
if ((count + 2 * m_degree) != m_knotsCount) {
if (m_knotVector) {
Free (m_knotVector);
}
m_knotsCount = count + 2 * m_degree;
m_knotVector = (dFloat64*) Alloc (m_knotsCount * sizeof (dFloat64));
}
for (int i = 0; i < (m_degree + 1); i ++) {
m_knotVector[i] = 0.0f;
m_knotVector[i + m_knotsCount - m_degree - 1] = 1.0f;
}
for (int i = 1; i < (count - 1); i ++) {
dFloat64 acc = 0.0f;
for (int j = 0; j < m_degree; j ++) {
acc += u[j + i - 1];
}
m_knotVector[m_degree + i] = acc / 3.0f;
}
//m_knotVector[4] = 1.0f/4.0f;
//m_knotVector[5] = 2.0f/4.0f;
//m_knotVector[6] = 3.0f/4.0f;
}
void SubmitConstraints(dFloat timestep, int threadIndex)
{
CustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
float invTimestep = 1.0f / timestep;
dMatrix matrix0;
dMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
if (m_anim_speed != 0.0f) // some animation to illustrate purpose
{
m_anim_time += timestep * m_anim_speed;
float a0 = sin(m_anim_time);
float a1 = m_anim_offset * 3.14f;
dVector axis(sin(a1), 0.0f, cos(a1));
//dVector axis (1,0,0);
m_target = dQuaternion(axis, a0 * 0.5f);
}
// measure error
dQuaternion q0(matrix0);
dQuaternion q1(matrix1);
dQuaternion qt0 = m_target * q1;
dQuaternion qErr = ((q0.DotProduct(qt0) < 0.0f) ? dQuaternion(-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3) : dQuaternion(q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;
float errorAngle = 2.0f * acos(dMax(-1.0f, dMin(1.0f, qErr.m_q0)));
dVector errorAngVel(0, 0, 0);
dMatrix basis;
if (errorAngle > 1.0e-10f) {
dVector errorAxis(qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
errorAxis = errorAxis.Scale(1.0f / dSqrt(errorAxis % errorAxis));
errorAngVel = errorAxis.Scale(errorAngle * invTimestep);
basis = dGrammSchmidt(errorAxis);
} else {
basis = dMatrix(qt0, dVector(0.0f, 0.0f, 0.0f, 1.0f));
}
dVector angVel0, angVel1;
NewtonBodyGetOmega(m_body0, (float*)&angVel0);
NewtonBodyGetOmega(m_body1, (float*)&angVel1);
dVector angAcc = (errorAngVel.Scale(m_reduceError) - (angVel0 - angVel1)).Scale(invTimestep);
// motor
for (int n = 0; n < 3; n++) {
// calculate the desired acceleration
dVector &axis = basis[n];
float relAccel = angAcc % axis;
NewtonUserJointAddAngularRow(m_joint, 0.0f, &axis[0]);
NewtonUserJointSetRowAcceleration(m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
}
}
dMatrix dGrammSchmidt (const dVector & dir)
{
dVector up;
dVector right;
dVector front (dir);
front = front.Scale (1.0f / dSqrt (front % front));
if (dAbs (front.m_z) > 0.577f)
right = front * dVector (-front.m_y, front.m_z, 0.0f);
else
right = front * dVector (-front.m_y, front.m_x, 0.0f);
right = right.Scale (1.0f / dSqrt (right % right));
up = right * front;
front.m_w = 0.0f;
up.m_w = 0.0f;
right.m_w = 0.0f;
return dMatrix (front, up, right, dVector (0.0f, 0.0f, 0.0f, 1.0f));
}
dQuaternion dQuaternion::IntegrateOmega (const dVector& omega, dFloat timestep) const
{
// this is correct
dQuaternion rotation (*this);
dFloat omegaMag2 = omega % omega;
const dFloat errAngle = 0.0125f * 3.141592f / 180.0f;
const dFloat errAngle2 = errAngle * errAngle;
if (omegaMag2 > errAngle2) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion deltaRotation (omegaAxis, omegaAngle);
rotation = rotation * deltaRotation;
rotation.Scale(1.0f / dSqrt (rotation.DotProduct (rotation)));
}
return rotation;
}
////////////////////////////////////////////////////////////////////////////////
/// Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
/// relative to body 1 and 2 initially) and then computes the constrained
/// rotational axis as the cross product of ax1 and ax2.
/// the sin and cos of the angle between axis 1 and 2 is computed, this comes
/// from dot and cross product rules.
///
/// @param ax1 Will contain the joint axis1 in world frame
/// @param ax2 Will contain the joint axis2 in world frame
/// @param axis Will contain the cross product of ax1 x ax2
/// @param sin_angle
/// @param cos_angle
////////////////////////////////////////////////////////////////////////////////
void
dxJointHinge2::getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axCross,
dReal &sin_angle, dReal &cos_angle) const
{
dMultiply0_331 (ax1, node[0].body->posr.R, axis1);
dMultiply0_331 (ax2, node[1].body->posr.R, axis2);
dCalcVectorCross3(axCross,ax1,ax2);
sin_angle = dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]);
cos_angle = dCalcVectorDot3 (ax1,ax2);
}
void dxCylinder::computeAABB()
{
const dMatrix3& R = final_posr->R;
const dVector3& pos = final_posr->pos;
dReal dOneMinusR2Square = (dReal)(REAL(1.0) - R[2]*R[2]);
dReal xrange = dFabs(R[2]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR2Square));
dReal dOneMinusR6Square = (dReal)(REAL(1.0) - R[6]*R[6]);
dReal yrange = dFabs(R[6]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR6Square));
dReal dOneMinusR10Square = (dReal)(REAL(1.0) - R[10]*R[10]);
dReal zrange = dFabs(R[10]*lz*REAL(0.5)) + radius * dSqrt(dMAX(REAL(0.0), dOneMinusR10Square));
aabb[0] = pos[0] - xrange;
aabb[1] = pos[0] + xrange;
aabb[2] = pos[1] - yrange;
aabb[3] = pos[1] + yrange;
aabb[4] = pos[2] - zrange;
aabb[5] = pos[2] + zrange;
}
dFloat ForceBodyAccelerationMichio (NewtonBody* const body)
{
dVector reactionforce (0.0f);
// calcualte accelration generate by all contacts
for (NewtonJoint* joint = NewtonBodyGetFirstContactJoint(body); joint; joint = NewtonBodyGetNextContactJoint(body, joint)) {
if (NewtonJointIsActive(joint)) {
for (void* contact = NewtonContactJointGetFirstContact(joint); contact; contact = NewtonContactJointGetNextContact(joint, contact)) {
dVector contactForce(0.0f);
NewtonMaterial* const material = NewtonContactGetMaterial(contact);
NewtonMaterialGetContactForce(material, body, &contactForce[0]);
reactionforce += contactForce;
}
}
}
dMatrix matrix;
dVector accel;
dVector veloc;
dFloat Ixx;
dFloat Iyy;
dFloat Izz;
dFloat mass;
NewtonBodyGetMass(body, &mass, &Ixx, &Iyy, &Izz);
NewtonBodyGetAcceleration(body, &accel[0]);
accel -= reactionforce.Scale (1.0f/mass);
//calculate centripetal acceleration here.
NewtonBodyGetMatrix(body, &matrix[0][0]);
dVector radius(matrix.m_posit.Scale(-1.0f));
radius.m_w = 0.0f;
dFloat radiusMag = dSqrt(radius.DotProduct3(radius));
dVector radiusDir (radius.Normalize());
NewtonBodyGetVelocity(body, &veloc[0]);
veloc += radiusDir.Scale(veloc.DotProduct3(radiusDir));
dVector centripetalAccel(veloc.DotProduct3(veloc) / radiusMag);
accel += centripetalAccel;
return dSqrt (accel.DotProduct3(accel));
}
void dRFrom2Axes (dMatrix3 R, dReal ax, dReal ay, dReal az,
dReal bx, dReal by, dReal bz)
{
dReal l,k;
dAASSERT (R);
l = dSqrt (ax*ax + ay*ay + az*az);
if (l <= REAL(0.0)) {
dDEBUGMSG ("zero length vector");
memset(R, 0, sizeof(dReal)*12);
return;
}
l = dRecip(l);
ax *= l;
ay *= l;
az *= l;
k = ax*bx + ay*by + az*bz;
bx -= k*ax;
by -= k*ay;
bz -= k*az;
l = dSqrt (bx*bx + by*by + bz*bz);
if (l <= REAL(0.0)) {
memset(R, 0, sizeof(dReal)*12);
dDEBUGMSG ("zero length vector");
return;
}
l = dRecip(l);
bx *= l;
by *= l;
bz *= l;
_R(0,0) = ax;
_R(1,0) = ay;
_R(2,0) = az;
_R(0,1) = bx;
_R(1,1) = by;
_R(2,1) = bz;
_R(0,2) = - by*az + ay*bz;
_R(1,2) = - bz*ax + az*bx;
_R(2,2) = - bx*ay + ax*by;
_R(0,3) = REAL(0.0);
_R(1,3) = REAL(0.0);
_R(2,3) = REAL(0.0);
}
dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
g->recomputePosr();
dxSphere *s = (dxSphere*) g;
dReal * pos = s->final_posr->pos;
return s->radius - dSqrt ((x-pos[0])*(x-pos[0]) +
(y-pos[1])*(y-pos[1]) +
(z-pos[2])*(z-pos[2]));
}
void dComplentaritySolver::dBodyState::IntegrateVelocity (dFloat timestep)
{
const dFloat D_MAX_ANGLE_STEP = dFloat (45.0f * 3.141592f / 180.0f);
const dFloat D_ANGULAR_TOL = dFloat (0.0125f * 3.141592f / 180.0f);
m_globalCentreOfMass += m_veloc.Scale (timestep);
while (((m_omega % m_omega) * timestep * timestep) > (D_MAX_ANGLE_STEP * D_MAX_ANGLE_STEP)) {
m_omega = m_omega.Scale (dFloat (0.8f));
}
// this is correct
dFloat omegaMag2 = m_omega % m_omega;
if (omegaMag2 > (D_ANGULAR_TOL * D_ANGULAR_TOL)) {
dFloat invOmegaMag = 1.0f / dSqrt (omegaMag2);
dVector omegaAxis (m_omega.Scale (invOmegaMag));
dFloat omegaAngle = invOmegaMag * omegaMag2 * timestep;
dQuaternion rotation (omegaAxis, omegaAngle);
dQuaternion rotMatrix (m_matrix);
rotMatrix = rotMatrix * rotation;
rotMatrix.Scale( 1.0f / dSqrt (rotMatrix.DotProduct (rotMatrix)));
m_matrix = dMatrix (rotMatrix, m_matrix.m_posit);
}
m_matrix.m_posit = m_globalCentreOfMass - m_matrix.RotateVector(m_localFrame.m_posit);
#ifdef _DEBUG
int j0 = 1;
int j1 = 2;
for (int i = 0; i < 3; i ++) {
dAssert (m_matrix[i][3] == 0.0f);
dFloat val = m_matrix[i] % m_matrix[i];
dAssert (dAbs (val - 1.0f) < 1.0e-5f);
dVector tmp (m_matrix[j0] * m_matrix[j1]);
val = tmp % m_matrix[i];
dAssert (dAbs (val - 1.0f) < 1.0e-5f);
j0 = j1;
j1 = i;
}
#endif
}
