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 }