XXXX () { dMatrix s; dFloat m = 2.0f; for (int i = 0; i < 3; i ++) { for (int j = 0; j < 3; j ++) { s[i][j] = m; m += (i + 1) + j; } } s.m_posit = dVector (1, 2, 3, 1); dMatrix matrix; dVector scale; dMatrix stretch; s.PolarDecomposition (matrix, scale, stretch); dMatrix s1 (matrix, scale, stretch); dMatrix xxx (dPitchMatrix (30.0f * 3.14159f / 180.0f) * dRollMatrix (30.0f * 3.14159f / 180.0f)); dMatrix xxxx (GetIdentityMatrix()); xxx[0] = xxx[0].Scale (-1.0f); dFloat mmm = (xxx[0] * xxx[1]) % xxx[2]; xxxx[0][0] = 3.0f; xxxx[1][1] = 3.0f; xxxx[2][2] = 4.0f; dMatrix xxx2 (xxx * xxxx); mmm = (xxx2[0] * xxx2[1]) % xxx2[2]; xxx2.PolarDecomposition (matrix, scale, stretch); s1 = dMatrix (matrix, scale, stretch); s1 = dMatrix (matrix, scale, stretch); }
dMatrix ARSByComplementaryFilterAndRotationMatrix::filter(const double gyro[3], const double accel[3], const double magn[3], const double &dt) { computeGyroAverage(average_gyro_, gyro, 10000, 10.); // 자이로 센서의 바이어스 값을 보정하기 위해 각속도 값에서 각속도 평균값을 뺀다. // 그리고 각도의 변화량을 회전행렬로 변환한다. dMatrix dR = RotationMatrix( _DEG2RAD*(gyro[0] - average_gyro_[0])*dt, _DEG2RAD*(gyro[1] - average_gyro_[1])*dt, _DEG2RAD*(gyro[2] - average_gyro_[2])*dt ); // 회전 행렬의 곱은 두 각도를 더하는 효과를 가진다. // R*dR은 R만큼 회전된 좌표계를 dR만큼 더 회전하게 된다. R_ = R_ * dR; dMatrix g = dMatrix(3, 1, accel); dMatrix m = dMatrix(3, 1, magn); // accel과 magn은 센서 좌표계를 기준으로 측정된 값이다. // 센서의 자세(R)을 곱해서 전역좌표계를 기준으로 한 값으로 바꿔준다. g = R_ * g; m = R_ * m; // 중력가속도 값이 1근처에 있을 때 이득(k1)이 큰 값을 가지도록 한다. double l = std::sqrt(accel[0]*accel[0] + accel[1]*accel[1] + accel[2]*accel[2]) - 1.; double sigma = 0.1; double k1 = 0.1 * std::exp(-l * l / (sigma * sigma)); #if defined ADIS_16405 double k2 = 0.1; // 각도의 보정량을 계산한다. double dPhi = g(2,0) ? std::atan(g(1,0) / g(2,0)) : 0.; double dTheta = (-1 < g(0,0) && g(0,0) < 1) ? std::asin(-g(0,0) / -1.) : 0.; double dPsi = -std::atan2(m(1,0), m(0,0)); #else double k2 = 0.; // 각도의 보정량을 계산한다. double dPhi = g(2,0) ? std::atan(g(1,0) / g(2,0)) : 0.; double dTheta = (-1 < g(0,0) && g(0,0) < 1) ? std::asin(-g(0,0) / -1.) : 0.; double dPsi = 0.; #endif adjust_rpy_[0] = dPhi; adjust_rpy_[1] = dTheta; adjust_rpy_[2] = dPsi; // 오일러각으로 계산한 보정량을 회전행렬로 변환한다. dMatrix aR = RotationMatrix(k1 * dPhi, k1 * dTheta, k2 * dPsi); // 계산된 보정량(aR)은 전역좌표계를 기준으로 계산된 값이다. // 그러므로 R의 앞에서 aR을 곱해야 한다. // R*aR 과 같이 쓰지 않도록 주의한다. R_ = aR * R_; return R_; }
dMatrix dMatrix::Transpose () const { return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, 0.0f), dVector (m_front.m_y, m_up.m_y, m_right.m_y, 0.0f), dVector (m_front.m_z, m_up.m_z, m_right.m_z, 0.0f), dVector (0.0f, 0.0f, 0.0f, 1.0f)); }
dMatrix dGetZeroMatrix () { return dMatrix (dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (0.0f, 0.0f, 0.0f, 0.0f), dVector (0.0f, 0.0f, 0.0f, 0.0f)); }
dMatrix dMatrix::Inverse () const { return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, 0.0f), dVector (m_front.m_y, m_up.m_y, m_right.m_y, 0.0f), dVector (m_front.m_z, m_up.m_z, m_right.m_z, 0.0f), dVector (- (m_posit % m_front), - (m_posit % m_up), - (m_posit % m_right), 1.0f)); }
dMatrix dGetIdentityMatrix() { return dMatrix (dVector (1.0f, 0.0f, 0.0f, 0.0f), dVector (0.0f, 1.0f, 0.0f, 0.0f), dVector (0.0f, 0.0f, 1.0f, 0.0f), dVector (0.0f, 0.0f, 0.0f, 1.0f)); }
// Returns a reference to a satTypeValueMap object after converting // from a geocentric reference system to a topocentric reference system. // // @param gData Data object holding the data. // satTypeValueMap& XYZ2NED::Process(satTypeValueMap& gData) throw(ProcessingException) { try { Matrix<double> neuMatrix; // Get the corresponding geometry/design matrix data Matrix<double> dMatrix(gData.getMatrixOfTypes(inputSet)); // Compute the base change. For convenience, we use the property: // Y = A*B => Y^T = (A*B)^T => Y^T = B^T * A^T neuMatrix = dMatrix*rotationMatrix; gData.insertMatrix(outputSet, neuMatrix); return gData; } catch(Exception& u) { // Throw an exception if something unexpected happens ProcessingException e( getClassName() + ":" + u.what() ); GPSTK_THROW(e); } } // End of method 'XYZ2NED::Process()'
void dMeshNodeInfo::BakeTransform (const dMatrix& transform) { dVector scale; dMatrix stretchMatrix; // dMatrix matrix (m_matrix * transform); // matrix.PolarDecomposition (m_matrix, scale, stretchMatrix); // matrix = dMatrix (GetIdentityMatrix(), scale, stretchMatrix); dMatrix tmp (m_matrix); dMatrix matrix (transform.Inverse4x4() * m_matrix * transform); matrix.PolarDecomposition (m_matrix, scale, stretchMatrix); matrix = transform * dMatrix (GetIdentityMatrix(), scale, stretchMatrix); int pointCount = NewtonMeshGetPointCount (m_mesh); int pointStride = NewtonMeshGetPointStrideInByte (m_mesh) / sizeof (dFloat); dFloat* const points = NewtonMeshGetPointArray (m_mesh); matrix.TransformTriplex(points, pointStride * sizeof (dFloat), points, pointStride * sizeof (dFloat), pointCount); dFloat* const normals = NewtonMeshGetNormalArray(m_mesh); dMatrix rotation (matrix.Inverse4x4().Transpose() * matrix); rotation.m_posit = dVector (0.0f, 0.0f, 0.0f, 1.0f); rotation.TransformTriplex(normals, pointStride * sizeof (dFloat), normals, pointStride * sizeof (dFloat), pointCount); int vertexCount = NewtonMeshGetVertexCount (m_mesh); int vertexStride = NewtonMeshGetVertexStrideInByte (m_mesh) / sizeof (dFloat); dFloat* const vertex = NewtonMeshGetVertexArray (m_mesh); matrix.TransformTriplex(vertex, vertexStride * sizeof (dFloat), vertex, vertexStride * sizeof (dFloat), vertexCount); }
dMatrix dMatrix::Transpose4X4 () const { return dMatrix (dVector (m_front.m_x, m_up.m_x, m_right.m_x, m_posit.m_x), dVector (m_front.m_y, m_up.m_y, m_right.m_y, m_posit.m_y), dVector (m_front.m_z, m_up.m_z, m_right.m_z, m_posit.m_z), dVector (m_front.m_w, m_up.m_w, m_right.m_w, m_posit.m_w)); }
vector<vector<double> > LinearAlgebra::getObservedEuclideanDistance(vector<vector<double> >& relAbundData){ int numSamples = relAbundData.size(); int numOTUs = relAbundData[0].size(); vector<vector<double> > dMatrix(numSamples); for(int i=0;i<numSamples;i++){ dMatrix[i].resize(numSamples); } for(int i=0;i<numSamples;i++){ for(int j=0;j<numSamples;j++){ double d = 0; for(int k=0;k<numOTUs;k++){ d += pow((relAbundData[i][k] - relAbundData[j][k]), 2.0000); } dMatrix[i][j] = pow(d, 0.50000); dMatrix[j][i] = dMatrix[i][j]; } } return dMatrix; }
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 dRollMatrix (dFloat ang) { dFloat cosAng; dFloat sinAng; sinAng = dSin (ang); cosAng = dCos (ang); return dMatrix (dVector ( cosAng, sinAng, 0.0f, 0.0f), dVector (-sinAng, cosAng, 0.0f, 0.0f), dVector ( 0.0f, 0.0f, 1.0f, 0.0f), dVector ( 0.0f, 0.0f, 0.0f, 1.0f)); }
void dLineNodeInfo::BakeTransform (const dMatrix& transform) { dVector scale; dMatrix stretchMatrix; dMatrix matrix (transform.Inverse4x4() * m_matrix * transform); matrix.PolarDecomposition (m_matrix, scale, stretchMatrix); matrix = transform * dMatrix (dGetIdentityMatrix(), scale, stretchMatrix); matrix.TransformTriplex (&m_curve.GetControlPointArray()[0].m_x, sizeof (dVector), &m_curve.GetControlPointArray()[0].m_x, sizeof (dVector), m_curve.GetControlPointCount()); }
void dMeshNodeInfo::BakeTransform (const dMatrix& transform) { dVector scale; dMatrix stretchMatrix; //dMatrix tmp (m_matrix); dMatrix matrix (transform.Inverse4x4() * m_matrix * transform); matrix.PolarDecomposition (m_matrix, scale, stretchMatrix); matrix = transform * dMatrix (dGetIdentityMatrix(), scale, stretchMatrix); NewtonMeshApplyTransform (m_mesh, &matrix[0][0]); }
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)); }
void DemoEntity::InterpolateMatrix (DemoEntityManager& world, dFloat param) { // read the data in a critical section to prevent race condition from oteh thread world.Lock(m_lock); dVector p0(m_curPosition); dVector p1(m_nextPosition); dQuaternion r0 (m_curRotation); dQuaternion r1 (m_nextRotation); // release the critical section world.Unlock(m_lock); dVector posit (p0 + (p1 - p0).Scale (param)); dQuaternion rotation (r0.Slerp(r1, param)); m_matrix = dMatrix (rotation, posit); }
dMatrix dMatrix::operator* (const dMatrix & B) const { const dMatrix & A = *this; return dMatrix (dVector (A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0], A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1], A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2], A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]), dVector (A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0], A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1], A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2], A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]), dVector (A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0], A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1], A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2], A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]), dVector (A[3][0] * B[0][0] + A[3][1] * B[1][0] + A[3][2] * B[2][0] + A[3][3] * B[3][0], A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1], A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2], A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3])); }
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 }
dMatrix DemoEntity::GetNextMatrix () const { return dMatrix (m_nextRotation, m_nextPosition); }
dNewtonHingeJoint::dNewtonHingeJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1) :dNewtonJoint(m_hinge) { SetJoint (new dCustomHinge (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL)); }
dNewtonBallAndSocketJoint::dNewtonBallAndSocketJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1) :dNewtonJoint(m_ballAndSocket) { SetJoint (new dCustomBallAndSocket (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL)); }
dNewtonCylindricalJoint::dNewtonCylindricalJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1) :dNewtonJoint(m_cylindrical) { SetJoint (new dCustomCorkScrew (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL)); }
dNewtonDoubleHinge::dNewtonDoubleHinge(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1) :dNewtonJoint(m_universal) { SetJoint (new dCustomDoubleHinge (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL)); }
dNewtonSliderJoint::dNewtonSliderJoint(const dFloat* const pinAndPivotFrame, dNewtonDynamicBody* const body0, dNewtonDynamicBody* const body1) :dNewtonJoint(m_slider) { SetJoint (new dCustomSlider (dMatrix(pinAndPivotFrame), body0->GetNewtonBody(), body1 ? body1->GetNewtonBody() : NULL)); }
void* dNewtonArticulationManager::dNewtonArticulationController::AddBone (dNewtonBody* const boneBody, const dFloat* const bindMatrix, void* const parentBone) { CustomArticulatedTransformController::dSkeletonBone* const parent = (CustomArticulatedTransformController::dSkeletonBone*) parentBone; CustomArticulatedTransformController::dSkeletonBone* const bone = m_controller->AddBone (boneBody->GetNewtonBody(), dMatrix (bindMatrix), parent); if (parent) { dNewtonBody* const parentBody = (dNewtonBody*) NewtonBodyGetUserData(parent->m_body); parentBody->AttachChild (boneBody); } // save the bone articulation in the body boneBody->SetBoneArticulation(bone); return bone; }
void CustomPlayerController::PostUpdate(dFloat timestep, int threadIndex) { dMatrix matrix; dQuaternion bodyRotation; dVector veloc(0.0f, 0.0f, 0.0f, 0.0f); dVector omega(0.0f, 0.0f, 0.0f, 0.0f); CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager(); NewtonWorld* const world = manager->GetWorld(); // apply the player motion, by calculation the desired plane linear and angular velocity manager->ApplyPlayerMove (this, timestep); // get the body motion state NewtonBodyGetMatrix(m_body, &matrix[0][0]); NewtonBodyGetVelocity(m_body, &veloc[0]); NewtonBodyGetOmega(m_body, &omega[0]); // integrate body angular velocity NewtonBodyGetRotation (m_body, &bodyRotation.m_q0); bodyRotation = bodyRotation.IntegrateOmega(omega, timestep); matrix = dMatrix (bodyRotation, matrix.m_posit); // integrate linear velocity dFloat normalizedTimeLeft = 1.0f; dFloat step = timestep * dSqrt (veloc % veloc) ; dFloat descreteTimeStep = timestep * (1.0f / D_DESCRETE_MOTION_STEPS); int prevContactCount = 0; CustomControllerConvexCastPreFilter castFilterData (m_body); NewtonWorldConvexCastReturnInfo prevInfo[PLAYER_CONTROLLER_MAX_CONTACTS]; dVector updir (matrix.RotateVector(m_upVector)); dVector scale; NewtonCollisionGetScale (m_upperBodyShape, &scale.m_x, &scale.m_y, &scale.m_z); //const dFloat radio = m_outerRadio * 4.0f; const dFloat radio = (m_outerRadio + m_restrainingDistance) * 4.0f; NewtonCollisionSetScale (m_upperBodyShape, m_height - m_stairStep, radio, radio); NewtonWorldConvexCastReturnInfo upConstratint; memset (&upConstratint, 0, sizeof (upConstratint)); upConstratint.m_normal[0] = m_upVector.m_x; upConstratint.m_normal[1] = m_upVector.m_y; upConstratint.m_normal[2] = m_upVector.m_z; upConstratint.m_normal[3] = m_upVector.m_w; for (int j = 0; (j < D_PLAYER_MAX_INTERGRATION_STEPS) && (normalizedTimeLeft > 1.0e-5f); j ++ ) { if ((veloc % veloc) < 1.0e-6f) { break; } dFloat timetoImpact; NewtonWorldConvexCastReturnInfo info[PLAYER_CONTROLLER_MAX_CONTACTS]; dVector destPosit (matrix.m_posit + veloc.Scale (timestep)); int contactCount = NewtonWorldConvexCast (world, &matrix[0][0], &destPosit[0], m_upperBodyShape, &timetoImpact, &castFilterData, CustomControllerConvexCastPreFilter::Prefilter, info, sizeof (info) / sizeof (info[0]), threadIndex); if (contactCount) { contactCount = manager->ProcessContacts (this, info, contactCount); } if (contactCount) { matrix.m_posit += veloc.Scale (timetoImpact * timestep); if (timetoImpact > 0.0f) { matrix.m_posit -= veloc.Scale (D_PLAYER_CONTACT_SKIN_THICKNESS / dSqrt (veloc % veloc)) ; } normalizedTimeLeft -= timetoImpact; dFloat speed[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; dFloat bounceSpeed[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; dVector bounceNormal[PLAYER_CONTROLLER_MAX_CONTACTS * 2]; for (int i = 1; i < contactCount; i ++) { dVector n0 (info[i-1].m_normal); for (int j = 0; j < i; j ++) { dVector n1 (info[j].m_normal); if ((n0 % n1) > 0.9999f) { info[i] = info[contactCount - 1]; i --; contactCount --; break; } } } int count = 0; if (!m_isJumping) { upConstratint.m_point[0] = matrix.m_posit.m_x; upConstratint.m_point[1] = matrix.m_posit.m_y; upConstratint.m_point[2] = matrix.m_posit.m_z; upConstratint.m_point[3] = matrix.m_posit.m_w; speed[count] = 0.0f; bounceNormal[count] = dVector (upConstratint.m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &upConstratint); count ++; } for (int i = 0; i < contactCount; i ++) { speed[count] = 0.0f; bounceNormal[count] = dVector (info[i].m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &info[i]); count ++; } for (int i = 0; i < prevContactCount; i ++) { speed[count] = 0.0f; bounceNormal[count] = dVector (prevInfo[i].m_normal); bounceSpeed[count] = CalculateContactKinematics(veloc, &prevInfo[i]); count ++; } dFloat residual = 10.0f; dVector auxBounceVeloc (0.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; (i < D_PLAYER_MAX_SOLVER_ITERATIONS) && (residual > 1.0e-3f); i ++) { residual = 0.0f; for (int k = 0; k < count; k ++) { dVector normal (bounceNormal[k]); dFloat v = bounceSpeed[k] - normal % auxBounceVeloc; dFloat x = speed[k] + v; if (x < 0.0f) { v = 0.0f; x = 0.0f; } if (dAbs (v) > residual) { residual = dAbs (v); } auxBounceVeloc += normal.Scale (x - speed[k]); speed[k] = x; } } dVector velocStep (0.0f, 0.0f, 0.0f, 0.0f); for (int i = 0; i < count; i ++) { dVector normal (bounceNormal[i]); velocStep += normal.Scale (speed[i]); } veloc += velocStep; dFloat velocMag2 = velocStep % velocStep; if (velocMag2 < 1.0e-6f) { dFloat advanceTime = dMin (descreteTimeStep, normalizedTimeLeft * timestep); matrix.m_posit += veloc.Scale (advanceTime); normalizedTimeLeft -= advanceTime / timestep; } prevContactCount = contactCount; memcpy (prevInfo, info, prevContactCount * sizeof (NewtonWorldConvexCastReturnInfo)); } else { matrix.m_posit = destPosit; matrix.m_posit.m_w = 1.0f; break; } } NewtonCollisionSetScale (m_upperBodyShape, scale.m_x, scale.m_y, scale.m_z); // determine if player is standing on some plane dMatrix supportMatrix (matrix); supportMatrix.m_posit += updir.Scale (m_sphereCastOrigin); if (m_isJumping) { dVector dst (matrix.m_posit); UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex); } else { step = dAbs (updir % veloc.Scale (timestep)); dFloat castDist = ((m_groundPlane % m_groundPlane) > 0.0f) ? m_stairStep : step; dVector dst (matrix.m_posit - updir.Scale (castDist * 2.0f)); UpdateGroundPlane (matrix, supportMatrix, dst, threadIndex); } // set player velocity, position and orientation NewtonBodySetVelocity(m_body, &veloc[0]); NewtonBodySetMatrix (m_body, &matrix[0][0]); }
dMatrix CustomPickBody::GetTargetMatrix () const { return dMatrix (m_targetRot, m_targetPosit); }
void dagPoseInfo::printDagPoseInfo(MObject& dagPoseNode, unsigned index) // // Description: // Given a dagPose and an index corresponding to a joint, print out // the matrix info for the joint. // Return: // None. // { MFnDependencyNode nDagPose(dagPoseNode); fprintf(file,"%s\n",nDagPose.name().asChar()); // construct plugs for this joints world and local matrices // MObject aWorldMatrix = nDagPose.attribute("worldMatrix"); MPlug pWorldMatrix(dagPoseNode,aWorldMatrix); pWorldMatrix.selectAncestorLogicalIndex(index,aWorldMatrix); MObject aMatrix = nDagPose.attribute("xformMatrix"); MPlug pMatrix(dagPoseNode,aMatrix); pMatrix.selectAncestorLogicalIndex(index,aMatrix); // get and print the world matrix data // MObject worldMatrix, xformMatrix; MStatus status = pWorldMatrix.getValue(worldMatrix); if (MS::kSuccess != status) { displayError("Problem retrieving world matrix."); } else { bool foundMatrix = 0; MFnMatrixData dMatrix(worldMatrix); MMatrix wMatrix = dMatrix.matrix(&status); if (MS::kSuccess == status) { foundMatrix = 1; unsigned jj,kk; fprintf(file,"worldMatrix\n"); for (jj = 0; jj < 4; ++jj) { for (kk = 0; kk < 4; ++kk) { double val = wMatrix(jj,kk); fprintf(file,"%f ",val); } fprintf(file,"\n"); } } if (!foundMatrix) { displayError("Error getting world matrix data."); } } // get and print the local matrix data // status = pMatrix.getValue(xformMatrix); if (MS::kSuccess != status) { displayError("Problem retrieving xform matrix."); } else { bool foundMatrix = 0; MFnMatrixData dMatrix(xformMatrix); if (dMatrix.isTransformation()) { MTransformationMatrix xform = dMatrix.transformation(&status); if (MS::kSuccess == status) { foundMatrix = 1; MMatrix xformAsMatrix = xform.asMatrix(); unsigned jj,kk; fprintf(file,"matrix\n"); for (jj = 0; jj < 4; ++jj) { for (kk = 0; kk < 4; ++kk) { double val = xformAsMatrix(jj,kk); fprintf(file,"%f ",val); } fprintf(file,"\n"); } } } if (!foundMatrix) { displayError("Error getting local matrix data."); } } }
dMatrix CustomKinematicController::GetTargetMatrix () const { return dMatrix (m_targetRot, m_targetPosit); }
void RenderJointsDebugInfo (NewtonWorld* const world, dFloat size) { glDisable(GL_TEXTURE_2D); glDisable (GL_LIGHTING); glBegin(GL_LINES); // this will go over the joint list twice, for (NewtonBody* body = NewtonWorldGetFirstBody(world); body; body = NewtonWorldGetNextBody(world, body)) { for (NewtonJoint* joint = NewtonBodyGetFirstJoint(body); joint; joint = NewtonBodyGetNextJoint(body, joint)) { NewtonJointRecord info; NewtonJointGetInfo (joint, &info); if (strcmp (info.m_descriptionType, "customJointNotInfo")) { // draw first frame dMatrix matrix0; NewtonBodyGetMatrix (info.m_attachBody_0, &matrix0[0][0]); matrix0 = dMatrix (&info.m_attachmenMatrix_0[0][0]) * matrix0; dVector o0 (matrix0.m_posit); dVector x (o0 + matrix0.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f))); glColor3f (1.0f, 0.0f, 0.0f); glVertex3f (o0.m_x, o0.m_y, o0.m_z); glVertex3f (x.m_x, x.m_y, x.m_z); dVector y (o0 + matrix0.RotateVector (dVector (0.0f, size, 0.0f, 0.0f))); glColor3f (0.0f, 1.0f, 0.0f); glVertex3f (o0.m_x, o0.m_y, o0.m_z); glVertex3f (y.m_x, y.m_y, y.m_z); dVector z (o0 + matrix0.RotateVector (dVector (0.0f, 0.0f, size, 0.0f))); glColor3f (0.0f, 0.0f, 1.0f); glVertex3f (o0.m_x, o0.m_y, o0.m_z); glVertex3f (z.m_x, z.m_y, z.m_z); // draw second frame dMatrix matrix1 (dGetIdentityMatrix()); if (info.m_attachBody_1) { NewtonBodyGetMatrix (info.m_attachBody_1, &matrix1[0][0]); } matrix1 = dMatrix (&info.m_attachmenMatrix_1[0][0]) * matrix1; dVector o1 (matrix1.m_posit); x = o1 + matrix1.RotateVector (dVector (size, 0.0f, 0.0f, 0.0f)); glColor3f (1.0f, 0.0f, 0.0f); glVertex3f (o1.m_x, o1.m_y, o1.m_z); glVertex3f (x.m_x, x.m_y, x.m_z); y = o1 + matrix1.RotateVector (dVector (0.0f, size, 0.0f, 0.0f)); glColor3f (0.0f, 1.0f, 0.0f); glVertex3f (o1.m_x, o1.m_y, o1.m_z); glVertex3f (y.m_x, y.m_y, y.m_z); z = o1 + matrix1.RotateVector (dVector (0.0f, 0.0f, size, 0.0f)); glColor3f (0.0f, 0.0f, 1.0f); glVertex3f (o1.m_x, o1.m_y, o1.m_z); glVertex3f (z.m_x, z.m_y, z.m_z); if (!strcmp (info.m_descriptionType, "limitballsocket")) { // draw the cone limit of this joint int steps = 12; dMatrix coneMatrix (dRollMatrix(info.m_maxAngularDof[1])); dMatrix ratationStep (dPitchMatrix(2.0f * 3.14151693f / steps)); dVector p0 (coneMatrix.RotateVector(dVector (size * 0.5f, 0.0f, 0.0f, 0.0f))); dVector q0 (matrix1.TransformVector(p0)); glColor3f (1.0f, 1.0f, 0.0f); for (int i = 0; i < (steps + 1); i ++) { dVector p1 (ratationStep.RotateVector(p0)); dVector q1 (matrix1.TransformVector(p1)); glVertex3f (o0.m_x, o0.m_y, o0.m_z); glVertex3f (q0.m_x, q0.m_y, q0.m_z); glVertex3f (q0.m_x, q0.m_y, q0.m_z); glVertex3f (q1.m_x, q1.m_y, q1.m_z); p0 = p1; q0 = q1; } } } } } glEnd(); }