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;
}
