glm::mat4 Transform::globalMatrix(){ if (mDirty.global) { mGlobalMatrix = localMatrix(); auto transformIterator = mParent; while (transformIterator) { mGlobalMatrix = transformIterator->localMatrix() * mGlobalMatrix; transformIterator = transformIterator->mParent; } mDirty.global = false; } return mGlobalMatrix; }
void BilinearTermToScalarTerm::get(MElement *ele, int npts, IntPt *GP, double &val) const { fullMatrix<double> localMatrix; bilterm.get(ele, npts, GP, localMatrix); val = localMatrix(0, 0); }
void dgCollisionInstance::SetGlobalScale (const dgVector& scale) { if ((dgAbsf (scale[0] - scale[1]) < dgFloat32 (1.0e-4f)) && (dgAbsf (scale[0] - scale[2]) < dgFloat32 (1.0e-4f))) { m_localMatrix.m_posit = m_localMatrix.m_posit.Scale3 (scale.m_x * m_invScale.m_x); SetScale (scale); } else { // extract the original local matrix dgMatrix localMatrix (m_aligmentMatrix * m_localMatrix); // create a new scale matrix localMatrix[0] = localMatrix[0].CompProduct4 (scale); localMatrix[1] = localMatrix[1].CompProduct4 (scale); localMatrix[2] = localMatrix[2].CompProduct4 (scale); localMatrix[3] = localMatrix[3].CompProduct4 (scale); localMatrix[3][3] = dgFloat32 (1.0f); // decompose into to align * scale * local localMatrix.PolarDecomposition (m_localMatrix, m_scale, m_aligmentMatrix); m_localMatrix = m_aligmentMatrix * m_localMatrix; m_aligmentMatrix = m_aligmentMatrix.Transpose(); bool isIdentity = true; for (dgInt32 i = 0; i < 3; i ++) { isIdentity &= dgAbsf (m_aligmentMatrix[i][i] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f); isIdentity &= dgAbsf (m_aligmentMatrix[3][i]) < dgFloat32 (1.0e-5f); } m_scaleType = isIdentity ? m_nonUniform : m_global; m_maxScale = dgMax(m_scale[0], m_scale[1], m_scale[2]); m_invScale = dgVector (dgFloat32 (1.0f) / m_scale[0], dgFloat32 (1.0f) / m_scale[1], dgFloat32 (1.0f) / m_scale[2], dgFloat32 (0.0f)); } }
void PostUpdate(dFloat timestep, int threadIndex) { int count = 1; void* nodes[32]; dMatrix parentMatrix[32]; nodes[0] = NewtonInverseDynamicsGetRoot(m_kinematicSolver); parentMatrix[0] = dGetIdentityMatrix(); NewtonWorld* const world = GetManager()->GetWorld(); DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world); while (count) { dMatrix matrix; count --; void* const rootNode = nodes[count]; NewtonBody* const body = NewtonInverseDynamicsGetBody(m_kinematicSolver, rootNode); NewtonBodyGetMatrix (body, &matrix[0][0]); dMatrix localMatrix (matrix * parentMatrix[count]); DemoEntity* const ent = (DemoEntity*)NewtonBodyGetUserData(body); dQuaternion rot(localMatrix); ent->SetMatrix(*scene, rot, localMatrix.m_posit); matrix = matrix.Inverse(); for (void* node = NewtonInverseDynamicsGetFirstChildNode(m_kinematicSolver, rootNode); node; node = NewtonInverseDynamicsGetNextChildNode(m_kinematicSolver, node)) { nodes[count] = node; parentMatrix[count] = matrix; count ++; } } }
void dgCollisionInstance::SetGlobalScale (const dgVector& scale) { // calculate current matrix dgMatrix matrix(dgGetIdentityMatrix()); matrix[0][0] = m_scale.m_x; matrix[1][1] = m_scale.m_y; matrix[2][2] = m_scale.m_z; matrix = m_aligmentMatrix * matrix * m_localMatrix; // extract the original local matrix dgMatrix transpose (matrix.Transpose()); dgVector globalScale (dgSqrt (transpose[0].DotProduct(transpose[0]).GetScalar()), dgSqrt (transpose[1].DotProduct(transpose[1]).GetScalar()), dgSqrt (transpose[2].DotProduct(transpose[2]).GetScalar()), dgFloat32 (1.0f)); dgVector invGlobalScale (dgFloat32 (1.0f) / globalScale.m_x, dgFloat32 (1.0f) / globalScale.m_y, dgFloat32 (1.0f) / globalScale.m_z, dgFloat32 (1.0f)); dgMatrix localMatrix (m_aligmentMatrix.Transpose() * m_localMatrix); localMatrix.m_posit = matrix.m_posit * invGlobalScale; dgAssert (localMatrix.m_posit.m_w == dgFloat32 (1.0f)); if ((dgAbs (scale[0] - scale[1]) < dgFloat32 (1.0e-4f)) && (dgAbs (scale[0] - scale[2]) < dgFloat32 (1.0e-4f))) { m_localMatrix = localMatrix; m_localMatrix.m_posit = m_localMatrix.m_posit * scale | dgVector::m_wOne; m_aligmentMatrix = dgGetIdentityMatrix(); SetScale (scale); } else { // create a new scale matrix localMatrix[0] = localMatrix[0] * scale; localMatrix[1] = localMatrix[1] * scale; localMatrix[2] = localMatrix[2] * scale; localMatrix[3] = localMatrix[3] * scale; localMatrix[3][3] = dgFloat32 (1.0f); // decompose into to align * scale * local localMatrix.PolarDecomposition (m_localMatrix, m_scale, m_aligmentMatrix); m_localMatrix = m_aligmentMatrix * m_localMatrix; m_aligmentMatrix = m_aligmentMatrix.Transpose(); dgAssert (m_localMatrix.TestOrthogonal()); dgAssert (m_aligmentMatrix.TestOrthogonal()); //dgMatrix xxx1 (dgGetIdentityMatrix()); //xxx1[0][0] = m_scale.m_x; //xxx1[1][1] = m_scale.m_y; //xxx1[2][2] = m_scale.m_z; //dgMatrix xxx (m_aligmentMatrix * xxx1 * m_localMatrix); bool isIdentity = true; for (dgInt32 i = 0; i < 3; i ++) { isIdentity &= dgAbs (m_aligmentMatrix[i][i] - dgFloat32 (1.0f)) < dgFloat32 (1.0e-5f); isIdentity &= dgAbs (m_aligmentMatrix[3][i]) < dgFloat32 (1.0e-5f); } m_scaleType = isIdentity ? m_nonUniform : m_global; m_maxScale = dgMax(m_scale[0], m_scale[1], m_scale[2]); m_invScale = dgVector (dgFloat32 (1.0f) / m_scale[0], dgFloat32 (1.0f) / m_scale[1], dgFloat32 (1.0f) / m_scale[2], dgFloat32 (0.0f)); } }
/** Returns the matrix computed concatenating this Transform's local matrix with the local matrices of all its parents. */ mat4 getComputedWorldMatrix() { mat4 world = localMatrix(); ref<ITransform> par = parent(); while(par) { world = par->localMatrix() * world; par = par->parent(); } return world; }
virtual void drawImplementation(RenderInfo& renderInfo) const { DebuggerCollisionMeshBuilder meshBuider (m_collision); glDisable (GL_LIGHTING); glDisable(GL_TEXTURE_2D); glBegin(GL_LINES); glColor3f(1.0f, 1.0f, 1.0f); dMatrix localMatrix (dGetIdentityMatrix()); m_collision->DebugRender (&localMatrix[0][0], &meshBuider); glEnd(); }
void dCustomCorkScrew::SubmitAngularRow(const dMatrix& matrix0, const dMatrix& matrix1, dFloat timestep) { dMatrix localMatrix(matrix0 * matrix1.Inverse()); dVector euler0; dVector euler1; localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw); dVector rollPin(dSin(euler0[1]), dFloat(0.0f), dCos(euler0[1]), dFloat(0.0f)); rollPin = matrix1.RotateVector(rollPin); NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, -euler0[2], &rollPin[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); // the joint angle can be determined by getting the angle between any two non parallel vectors m_curJointAngle.Update(euler0.m_x); // save the current joint Omega dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } m_angularOmega = (omega0 - omega1).DotProduct3(matrix1.m_front); if (m_options.m_option2) { if (m_options.m_option3) { dCustomCorkScrew::SubmitConstraintLimitSpringDamper(matrix0, matrix1, timestep); } else { dCustomCorkScrew::SubmitConstraintLimits(matrix0, matrix1, timestep); } } else if (m_options.m_option3) { dCustomCorkScrew::SubmitConstraintSpringDamper(matrix0, matrix1, timestep); } else if (m_angularFriction != 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0, &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowAcceleration(m_joint, -m_angularOmega / timestep); NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction); } }
void Custom6DOF::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); // add the linear limits const dVector& p0 = matrix0.m_posit; const dVector& p1 = matrix1.m_posit; dVector dp (p0 - p1); for (int i = 0; i < 3; i ++) { if ((m_minLinearLimits[i] == 0.0f) && (m_maxLinearLimits[i] == 0.0f)) { NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); } else { // it is a limited linear dof, check if it pass the limits dFloat dist = dp.DotProduct3(matrix1[i]); if (dist > m_maxLinearLimits[i]) { dVector q1 (p1 + matrix1[i].Scale (m_maxLinearLimits[i])); // clamp the error, so the not too much energy is added when constraint violation occurs dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]); if (maxDist > D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) { q1 = p0 - matrix1[i].Scale(D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION); } NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the object to return but not to kick going forward NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f); } else if (dist < m_minLinearLimits[i]) { dVector q1 (p1 + matrix1[i].Scale (m_minLinearLimits[i])); // clamp the error, so the not too much energy is added when constraint violation occurs dFloat maxDist = (p0 - q1).DotProduct3(matrix1[i]); if (maxDist < -D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION) { q1 = p0 - matrix1[i].Scale(-D_6DOF_ANGULAR_MAX_LINEAR_CORRECTION); } NewtonUserJointAddLinearRow (m_joint, &p0[0], &q1[0], &matrix0[i][0]); NewtonUserJointSetRowStiffness (m_joint, 1.0f); // allow the object to return but not to kick going forward NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f); } } } dVector euler0(0.0f); dVector euler1(0.0f); dMatrix localMatrix (matrix0 * matrix1.Inverse()); localMatrix.GetEulerAngles(euler0, euler1); AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch); AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch); if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) { euler0 = euler1; } dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f); //dTrace (("(%f %f %f) (%f %f %f)\n", m_pitch.m_angle * 180.0f / 3.141592f, m_yaw.m_angle * 180.0f / 3.141592f, m_roll.m_angle * 180.0f / 3.141592f, euler0.m_x * 180.0f / 3.141592f, euler0.m_y * 180.0f / 3.141592f, euler0.m_z * 180.0f / 3.141592f)); bool limitViolation = false; for (int i = 0; i < 3; i ++) { if (euler[i] < m_minAngularLimits[i]) { limitViolation = true; euler[i] = m_minAngularLimits[i]; } else if (euler[i] > m_maxAngularLimits[i]) { limitViolation = true; euler[i] = m_maxAngularLimits[i]; } } if (limitViolation) { //dMatrix pyr (dPitchMatrix(m_pitch.m_angle) * dYawMatrix(m_yaw.m_angle) * dRollMatrix(m_roll.m_angle)); dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2])); dMatrix baseMatrix (p0y0r0 * matrix1); dMatrix rotation (matrix0.Inverse() * baseMatrix); dQuaternion quat (rotation); if (quat.m_q0 > dFloat (0.99995f)) { //dVector p0 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH)); //dVector p1 (matrix0[3] + baseMatrix[1].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &baseMatrix[2][0]); //NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f); //dVector q0 (matrix0[3] + baseMatrix[0].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[1][0]); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &baseMatrix[2][0]); } else { dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); dVector q0 (matrix0[3] + basis[1].Scale (MIN_JOINT_PIN_LENGTH)); dVector q1 (matrix0[3] + rotation.RotateVector(basis[1].Scale (MIN_JOINT_PIN_LENGTH))); NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &basis[2][0]); NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f); //dVector q0 (matrix0[3] + basis[0].Scale (MIN_JOINT_PIN_LENGTH)); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[1][0]); //NewtonUserJointAddLinearRow (m_joint, &q0[0], &q0[0], &basis[2][0]); } } }
int main(int argc, char *argv[]) { #ifdef HAVE_MPI MPI_Init(&argc,&argv); Tpetra::MpiComm<OrdinalType, ScalarType> Comm(MPI_COMM_WORLD); #else Tpetra::SerialComm<OrdinalType, ScalarType> Comm; #endif if (Comm.getNumImages() != 2) { cout << "This example must be ran with two processors" << endl; #ifdef HAVE_MPI MPI_Finalize(); #endif exit(EXIT_SUCCESS); } // Get zero and one for the OrdinalType OrdinalType const OrdinalZero = Teuchos::ScalarTraits<OrdinalType>::zero(); OrdinalType const OrdinalOne = Teuchos::ScalarTraits<OrdinalType>::one(); // Get zero and one for the ScalarType ScalarType const ScalarOne = Teuchos::ScalarTraits<ScalarType>::one(); ScalarType const ScalarZero = Teuchos::ScalarTraits<ScalarType>::zero(); OrdinalType indexBase = OrdinalZero; // Creation of a platform #ifdef HAVE_MPI const Tpetra::MpiPlatform <OrdinalType, OrdinalType> platformE(MPI_COMM_WORLD); const Tpetra::MpiPlatform <OrdinalType, ScalarType> platformV(MPI_COMM_WORLD); #else const Tpetra::SerialPlatform <OrdinalType, OrdinalType> platformE; const Tpetra::SerialPlatform <OrdinalType, ScalarType> platformV; #endif // These are the `elements' assigned to this image // NOTE: This element is NOT a finite element, instead it is // a component of the ElementSpace (that is, the distribution of // vertices). FiniteElements values are defined later on. // // NOTATION: // - Element -> an entity of the space // - Vertex -> finite element vertex // - FiniteElement -> finite element, composed by two Vertices const OrdinalType NumMyVertices = OrdinalOne * 3; vector<OrdinalType> MyGlobalVertices(NumMyVertices); if (Comm.getMyImageID() == 0) { for (OrdinalType i = OrdinalZero ; i < NumMyVertices ; ++i) MyGlobalVertices[OrdinalZero + i] = OrdinalZero + i; } else { for (OrdinalType i = OrdinalZero ; i < NumMyVertices ; ++i) MyGlobalVertices[OrdinalZero + i] = OrdinalZero + i + 3; } Tpetra::ElementSpace<OrdinalType> VertexSpace(-OrdinalOne, NumMyVertices, MyGlobalVertices, indexBase, platformE); Tpetra::VectorSpace<OrdinalType, ScalarType> VectorVertexSpace(VertexSpace, platformV); const OrdinalType NumMyPaddedVertices = OrdinalOne * 4; vector<OrdinalType> MyGlobalPaddedVertices(NumMyPaddedVertices); // Vector BoundaryVertices contains a flag that specifies // whether the node is a Dirichlet node or not vector<bool> BoundaryVertices(NumMyVertices); // Setting the elements of the connectivity if (Comm.getMyImageID() == 0) { MyGlobalPaddedVertices[0] = OrdinalZero; MyGlobalPaddedVertices[1] = OrdinalOne; MyGlobalPaddedVertices[2] = OrdinalOne * 2; MyGlobalPaddedVertices[3] = OrdinalOne * 3; // ghost node BoundaryVertices[0] = true; BoundaryVertices[1] = false; BoundaryVertices[2] = false; } else { MyGlobalPaddedVertices[0] = OrdinalOne * 3; MyGlobalPaddedVertices[1] = OrdinalOne * 4; MyGlobalPaddedVertices[2] = OrdinalOne * 5; MyGlobalPaddedVertices[3] = OrdinalOne * 2; // ghost node BoundaryVertices[0] = false; BoundaryVertices[1] = false; BoundaryVertices[2] = true; } // This space is needed to import the values of coordinates corresponding // to ghost elements. Tpetra::ElementSpace<OrdinalType> PaddedVertexSpace(-OrdinalOne, NumMyPaddedVertices, MyGlobalPaddedVertices, indexBase, platformE); Tpetra::VectorSpace<OrdinalType, ScalarType> VectorPaddedVertexSpace(PaddedVertexSpace, platformV); // This is the connectivity, in local numbering const OrdinalType NumVerticesPerFiniteElement = 2; const OrdinalType NumDimensions = 2; const OrdinalType NumMyElements = OrdinalOne * 3; Teuchos::SerialDenseMatrix<OrdinalType, OrdinalType> Connectivity(NumMyElements, NumVerticesPerFiniteElement); // this contains the coordinates Tpetra::Vector<OrdinalType, ScalarType> Coord(VectorPaddedVertexSpace); if (Comm.getMyImageID() == 0) { Connectivity(0,0) = 0; Connectivity(0,1) = 1; Connectivity(1,0) = 1; Connectivity(1,1) = 2; Connectivity(2,0) = 2; Connectivity(2,1) = 3; Coord[0] = ScalarZero; Coord[1] = ScalarOne; Coord[2] = ScalarOne * 2; Coord[3] = ScalarOne * 3; // ghost node is last } else { Connectivity(0,0) = 0; Connectivity(0,1) = 1; Connectivity(1,0) = 1; Connectivity(1,1) = 2; Connectivity(2,0) = 3; Connectivity(2,1) = 0; Coord[0] = ScalarOne * 3; Coord[1] = ScalarOne * 4; Coord[2] = ScalarOne * 5; Coord[3] = ScalarOne * 2; // ghost node is last } // Setup the matrix Teuchos::SerialDenseMatrix<OrdinalType, ScalarType> localMatrix(NumVerticesPerFiniteElement, NumVerticesPerFiniteElement); Tpetra::CisMatrix<OrdinalType,ScalarType> matrix(VectorVertexSpace); // Loop over all the (locally owned) elements for (OrdinalType FEID = OrdinalZero ; FEID < NumMyElements ; ++FEID) { vector<OrdinalType> LIDs(NumVerticesPerFiniteElement), GIDs(NumVerticesPerFiniteElement); // Get the local and global ID of this element's vertices for (OrdinalType i = OrdinalZero ; i < NumVerticesPerFiniteElement ; ++i) { LIDs[i] = Connectivity(FEID, i); GIDs[i] = PaddedVertexSpace.getGID(LIDs[i]); } // get the coordinates of all nodes in the element vector<ScalarType> x(NumVerticesPerFiniteElement); for (OrdinalType i = 0 ; i < NumDimensions ; ++i) { x[i] = Coord[LIDs[i]]; } // build the local matrix, in this case A_loc; // this is specific to this 1D Laplace, and does not use // coordinates. localMatrix(0, 0) = ScalarOne; localMatrix(1, 0) = -ScalarOne; localMatrix(0, 1) = -ScalarOne; localMatrix(1, 1) = ScalarOne; // submit entries of localMatrix into the matrix for (OrdinalType LRID = OrdinalZero ; LRID < NumVerticesPerFiniteElement ; ++LRID) { OrdinalType LocalRow = LIDs[LRID]; // We skip non-locally owned vertices if (LocalRow < NumMyVertices) { // If the node is a boundary node, then put 1 on the diagonal if (BoundaryVertices[LocalRow]) { matrix.submitEntry(Tpetra::Insert, GIDs[LRID], ScalarOne, GIDs[LRID]); } else { // otherwise add the entire row for (OrdinalType LCID = OrdinalZero ; LCID < NumVerticesPerFiniteElement ; ++LCID) { matrix.submitEntry(Tpetra::Add, GIDs[LRID], localMatrix(LRID,LCID), GIDs[LCID]); } } } } } matrix.fillComplete(); cout << matrix; #ifdef HAVE_MPI MPI_Finalize() ; #endif return(EXIT_SUCCESS); }
void CustomControlledBallAndSocket::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); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); #if 0 dVector euler0; dVector euler1; dMatrix localMatrix (matrix0 * matrix1.Inverse()); localMatrix.GetEulerAngles(euler0, euler1); AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch); AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch); if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) { euler0 = euler1; } dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f); for (int i = 0; i < 3; i ++) { dFloat error = m_targetAngles[i] - euler[i]; if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) { dFloat angularStep = dSign(error) * m_angulaSpeed * timestep; if (angularStep > 0.0f) { if (angularStep > error) { angularStep = error * 0.5f; } } else { if (angularStep < error) { angularStep = error * 0.5f; } } euler[i] = euler[i] + angularStep; } } dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2])); dMatrix baseMatrix (p0y0r0 * matrix1); dMatrix rotation (matrix0.Inverse() * baseMatrix); dQuaternion quat (rotation); if (quat.m_q0 > dFloat (0.99995f)) { dVector euler0; dVector euler1; rotation.GetEulerAngles(euler0, euler1); NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]); NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]); NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]); } else { dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]); } #else matrix1 = m_targetRotation * matrix1; dQuaternion localRotation(matrix1 * matrix0.Inverse()); if (localRotation.DotProduct(m_targetRotation) < 0.0f) { localRotation.Scale(-1.0f); } dFloat angle = 2.0f * dAcos(localRotation.m_q0); dFloat angleStep = m_angulaSpeed * timestep; if (angleStep < angle) { dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f)); axis = axis.Scale(1.0f / dSqrt(axis % axis)); // localRotation = dQuaternion(axis, angleStep); } dVector axis (matrix1.m_front * matrix1.m_front); dVector axis1 (matrix1.m_front * matrix1.m_front); //dFloat sinAngle; //dFloat cosAngle; //CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle); //float xxxx = dAtan2(sinAngle, cosAngle); //float xxxx1 = dAtan2(sinAngle, cosAngle); dQuaternion quat(localRotation); if (quat.m_q0 > dFloat(0.99995f)) { // dAssert (0); /* dVector euler0; dVector euler1; rotation.GetEulerAngles(euler0, euler1); NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]); NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]); NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]); */ } else { dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]); } #endif }
void renderTo(const ICamera* cam, const math::Matrix& parent, const Lights& lights) const override { math::Matrix accumulated = parent * localMatrix(); for (auto && child : m_children) child->renderTo(cam, accumulated, lights); }
void dCustomBallAndSocket::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); SubmitLinearRows(0x07, matrix0, matrix1); const dVector& coneDir0 = matrix0.m_front; const dVector& coneDir1 = matrix1.m_front; dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0); dMatrix coneRotation(dGetIdentityMatrix()); dVector lateralDir(matrix0.m_up); if (cosAngleCos < 0.9999f) { lateralDir = coneDir1.CrossProduct(coneDir0); dFloat mag2 = lateralDir.DotProduct3(lateralDir); if (mag2 > 1.0e-4f) { lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2)); coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit); } else { lateralDir = matrix0.m_up.Scale (-1.0f); coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit); } } dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } dVector relOmega(omega0 - omega1); // do twist angle calculations dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse()); dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1])); if (m_options.m_option0) { if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) { NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); } else { if (m_options.m_option1) { // TODO spring option dAssert (0); } else { SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep); } } } else if (m_options.m_option1) { // TODO spring option dAssert (0); } else if (m_twistFriction > 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction); } // do twist cone angle calculations if (m_options.m_option2) { if ((m_maxConeAngle == 0.0f)) { dMatrix localMatrix(matrix0 * matrix1.Inverse()); dVector euler0; dVector euler1; localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw); NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); } else { if (m_options.m_option3) { // TODO spring option dAssert(0); } else { dFloat jointOmega = relOmega.DotProduct3(lateralDir); dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f))); dFloat coneAngle = currentAngle + jointOmega * timestep; if (coneAngle >= m_maxConeAngle) { //dQuaternion rot(lateralDir, coneAngle); //dVector frontDir(rot.RotateVector(coneDir1)); //dVector upDir(lateralDir.CrossProduct(frontDir)); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); const dFloat invtimestep = 1.0f / timestep; const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep; const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep; NewtonUserJointSetRowAcceleration(m_joint, stopAccel); } else if (m_coneFriction != 0) { NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); } } } } else if (m_options.m_option3) { // TODO spring option dAssert(0); } else if (m_coneFriction > 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); } }
AffineTransform SVGTransformable::getScreenCTM(const SVGElement* element) const { AffineTransform ctm = SVGLocatable::getScreenCTM(element); return localMatrix() * ctm; }