void
ColumnMajorMatrixTest::exp()
{
ColumnMajorMatrix matrix1(3,3), matrix_exp1(3,3);
ColumnMajorMatrix matrix2(3,3), matrix_exp2(3,3);
Real err = 1.0e-10;
Real e = 2.71828182846;
Real e1 = (e*e*e*e - e)/3, e2 = (2*e + e*e*e*e)/3;
matrix1(0,0) = 2.0;
matrix1(0,1) = 1.0;
matrix1(0,2) = 1.0;
matrix1(1,0) = 1.0;
matrix1(1,1) = 2.0;
matrix1(1,2) = 1.0;
matrix1(2,0) = 1.0;
matrix1(2,1) = 1.0;
matrix1(2,2) = 2.0;
matrix1.exp(matrix_exp1);
CPPUNIT_ASSERT( std::abs(matrix_exp1(0,0) - e2) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp1(0,1) - e1) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp1(0,2) - e1) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp1(1,0) - e1) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp1(1,1) - e2) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp1(1,2) - e1) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp1(2,0) - e1) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp1(2,1) - e1) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp1(2,2) - e2) < err );
matrix2(0,0) = 1.0;
matrix2(0,1) = 1.0;
matrix2(0,2) = 0.0;
matrix2(1,0) = 0.0;
matrix2(1,1) = 0.0;
matrix2(1,2) = 2.0;
matrix2(2,0) = 0.0;
matrix2(2,1) = 0.0;
matrix2(2,2) = -1.0;
matrix2.exp(matrix_exp2);
CPPUNIT_ASSERT( std::abs(matrix_exp2(0,0) - e) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp2(0,1) - (e-1)) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp2(0,2) - (-(-(e*e)+(2*e)-1)/e)) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp2(1,0) - 0.0) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp2(1,1) - 1) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp2(1,2) - (2*(e-1)/e)) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp2(2,0) - 0.0) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp2(2,1) - 0.0) < err );
CPPUNIT_ASSERT( std::abs(matrix_exp2(2,2) - (1/e)) < err );
}
void MatrixTest::testMatrixMatrixOperations ()
{
Matrix<2,3,int> lMatrix;
Matrix<3,2,int> rMatrix;
Matrix<2,2,int> result(0);
assignList(lMatrix) = 1, 2, 3,
4, 5, 6;
assignList(rMatrix) = 6, 5,
4, 3,
2, 1;
// Matrix matrix multiplication
multiply (lMatrix, rMatrix, result);
validateEquals (result(0,0), 20);
validateEquals (result(0,1), 14);
validateEquals (result(1,0), 56);
validateEquals (result(1,1), 41);
// Bitwise comparison
Matrix<2,3,int> matrixA(1);
Matrix<2,3,int> matrixB(2);
validate (matrixA == matrixA);
validate (! (matrixA == matrixB));
// Test equalsReturnIndex
Matrix<2,3,double> matrix1(1);
Matrix<2,3,double> matrix2(2);
int i=equalsReturnIndex(matrix1,matrix2);
validateEquals(i,0);
}
void ShowJointInfo(const NewtonCustomJoint* joint)
{
NewtonJointRecord info;
if (showContacts) {
joint->GetInfo (&info);
dMatrix bodyMatrix0;
NewtonBodyGetMatrix (info.m_attachBody_0, &bodyMatrix0[0][0]);
dMatrix matrix0 (*((dMatrix*) &info.m_attachmenMatrix_0[0]));
matrix0 = matrix0 * bodyMatrix0;
DebugDrawLine (matrix0.m_posit, matrix0.m_posit + matrix0.m_front, dVector (1, 0, 0));
DebugDrawLine (matrix0.m_posit, matrix0.m_posit + matrix0.m_up, dVector (0, 1, 0));
DebugDrawLine (matrix0.m_posit, matrix0.m_posit + matrix0.m_right, dVector (0, 0, 1));
dMatrix bodyMatrix1;
NewtonBodyGetMatrix (info.m_attachBody_1, &bodyMatrix1[0][0]);
dMatrix matrix1 (*((dMatrix*) &info.m_attachmenMatrix_1[0]));
matrix1 = matrix1 * bodyMatrix1;
DebugDrawLine (matrix1.m_posit, matrix1.m_posit + matrix1.m_front, dVector (1, 0, 0));
DebugDrawLine (matrix1.m_posit, matrix1.m_posit + matrix1.m_up, dVector (0, 1, 0));
DebugDrawLine (matrix1.m_posit, matrix1.m_posit + matrix1.m_right, dVector (0, 0, 1));
}
}
CustomUniversal::CustomUniversal(const dMatrix& pinsAndPivoFrame, NewtonBody* child, NewtonBody* parent)
:NewtonCustomJoint(6, child, parent)
{
dMatrix matrix0;
m_limit_0_On = true;
m_angularMotor_0_On = false;
m_angularDamp_0 = 0.5f;
m_angularAccel_0 = -4.0f;
m_minAngle_0 = -45.0f * 3.141592f / 180.0f;
m_maxAngle_0 = 45.0f * 3.141592f / 180.0f;
m_limit_1_On = true;
m_angularMotor_1_On = false;
m_angularDamp_1 = 0.3f;
m_angularAccel_1 = -4.0f;
m_minAngle_1 = -45.0f * 3.141592f / 180.0f;
m_maxAngle_1 = 45.0f * 3.141592f / 180.0f;
// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
dMatrix matrix1 (GetIdentityMatrix());
if (m_body1) {
NewtonBodyGetMatrix(m_body1, &matrix1[0][0]);
}
// calculate the relative matrix of the pin and pivot on each body
m_localMatrix0 = pinsAndPivoFrame * matrix0.Inverse();
m_localMatrix1 = pinsAndPivoFrame * matrix1.Inverse();
}
void dgCollisionInstance::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
// m_childShape->CalcAABB (matrix, p0, p1);
// p0 = (matrix.m_posit + (p0 - matrix.m_posit).CompProduct4(m_maxScale) - m_padding) & dgVector::m_triplexMask;
// p1 = (matrix.m_posit + (p1 - matrix.m_posit).CompProduct4(m_maxScale) + m_padding) & dgVector::m_triplexMask;
switch (m_scaleType)
{
case m_unit:
{
m_childShape->CalcAABB (matrix, p0, p1);
p0 -= m_padding;
p1 += m_padding;
break;
}
case m_uniform:
case m_nonUniform:
{
dgMatrix matrix1 (matrix);
matrix1[0] = matrix1[0].Scale4(m_scale.m_x);
matrix1[1] = matrix1[1].Scale4(m_scale.m_y);
matrix1[2] = matrix1[2].Scale4(m_scale.m_z);
m_childShape->CalcAABB (matrix1, p0, p1);
p0 -= m_padding;
p1 += m_padding;
break;
}
case m_global:
default:
{
dgMatrix matrix1 (matrix);
matrix1[0] = matrix1[0].Scale4(m_scale.m_x);
matrix1[1] = matrix1[1].Scale4(m_scale.m_y);
matrix1[2] = matrix1[2].Scale4(m_scale.m_z);
m_childShape->CalcAABB (m_aligmentMatrix * matrix1, p0, p1);
p0 -= m_padding;
p1 += m_padding;
break;
}
}
dgAssert (p0.m_w == dgFloat32 (0.0f));
dgAssert (p1.m_w == dgFloat32 (0.0f));
}
void dgCollisionInstance::CalcAABB (const dgMatrix& matrix, dgVector& p0, dgVector& p1) const
{
switch (m_scaleType)
{
case m_unit:
{
m_childShape->CalcAABB (matrix, p0, p1);
p0 -= m_padding;
p1 += m_padding;
break;
}
case m_uniform:
case m_nonUniform:
{
dgMatrix matrix1 (matrix);
matrix1[0] = matrix1[0].Scale(m_scale.m_x);
matrix1[1] = matrix1[1].Scale(m_scale.m_y);
matrix1[2] = matrix1[2].Scale(m_scale.m_z);
m_childShape->CalcAABB (matrix1, p0, p1);
p0 -= m_padding;
p1 += m_padding;
break;
}
case m_global:
default:
{
dgMatrix matrix1 (matrix);
matrix1[0] = matrix1[0].Scale(m_scale.m_x);
matrix1[1] = matrix1[1].Scale(m_scale.m_y);
matrix1[2] = matrix1[2].Scale(m_scale.m_z);
m_childShape->CalcAABB (m_aligmentMatrix * matrix1, p0, p1);
p0 -= m_padding;
p1 += m_padding;
break;
}
}
dgAssert (p0.m_w == dgFloat32 (0.0f));
dgAssert (p1.m_w == dgFloat32 (0.0f));
}
void DemoSoundListener::PostUpdate (const NewtonWorld* const world, dFloat timestep)
{
DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world);
DemoCamera* const camera = scene->GetCamera();
dMatrix matrix0 (camera->GetCurrentMatrix ());
dMatrix matrix1 (camera->GetNextMatrix ());
dVector veloc ((matrix1.m_posit - matrix0.m_posit).Scale (1.0f / timestep));
//printf ("%f %f %f %f\n", veloc.m_x, veloc.m_y, veloc.m_z, timestepInSecunds);
UpdateListener (matrix1.m_posit, veloc, matrix0.m_front, matrix0.m_up);
dSoundManager::Update();
}
void CustomPathFollow::SubmitConstraints (dFloat timestep, int threadIndex)
{
dMatrix matrix0;
// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
matrix0 = m_localMatrix0 * matrix0;
dMatrix matrix1 (EvalueCurve (matrix0.m_posit));
// Restrict the movement on the pivot point along all tree the normal and binormal of the path
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);
// get a point along the ping axis at some reasonable large distance from the pivot
dVector q0 (p0 + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector q1 (p1 + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// two more constraints rows to inforce the normal and binormal
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &q0[0], &q1[0], &matrix0.m_right[0]);
// Julio: at this point the path follow will maintaing the pivot fixed to the path and will allow it to slide freelly
// it will also spin around the path tangent, if more control is needed then more constraion rows need to be submited
// for example friction along the oath tangent be added to make more realistic behavior
// or anothet point contration along the binormal to make no spin around the path tangent
/*
// example of ading friction
dVector veloc0;
dVector omega0;
NewtonBodyGetVelocity(m_body0, &veloc0[0]);
NewtonBodyGetOmega(m_body0, &omega0[0]);
veloc0 += omega0 * (matrix0.m_posit - p0);
dFloat relAccel;
relAccel = - (veloc0 % matrix0.m_front) / timestep;
#define MaxFriction 10.0f
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &matrix0.m_front[0]);
NewtonUserJointSetRowAcceleration (m_joint, relAccel);
NewtonUserJointSetRowMinimumFriction (m_joint, -MaxFriction);
NewtonUserJointSetRowMaximumFriction(m_joint, MaxFriction);
*/
}
void MapsMerge::GaleShapleyMatcherStrategy::testGaleShapleyAlgorithm() {
// Initialization
const int dimension = 4;
int prefer[2 * dimension][dimension] = {
{7, 5, 6, 4},
{5, 4, 6, 7},
{4, 5, 6, 7},
{4, 5, 6, 7},
{0, 1, 2, 3},
{0, 1, 2, 3},
{0, 1, 2, 3},
{0, 1, 2, 3},
};
vector<vector<int>> matrix1(dimension, vector<int>(dimension));
vector<vector<int>> matrix2(dimension, vector<int>(dimension));
cout << "Before initialization" << endl;
//for (int i = 0; i < dimension; i++) {
// matrix1[i].assign(&prefer[i][0], &prefer[i][dimension]);
// matrix1[i + dimension].assign(&prefer[i + dimension][0], &prefer[i + dimension][dimension]);
//}
matrix1[0][0] = 3; matrix1[0][1] = 1; matrix1[0][2] = 2; matrix1[0][3] = 0;
matrix1[1][0] = 1; matrix1[1][1] = 0; matrix1[1][2] = 2; matrix1[1][3] = 3;
matrix1[2][0] = 0; matrix1[2][1] = 1; matrix1[2][2] = 2; matrix1[2][3] = 3;
matrix1[3][0] = 0; matrix1[3][1] = 1; matrix1[3][2] = 2; matrix1[3][3] = 3;
matrix2[0][0] = 0; matrix2[0][1] = 1; matrix2[0][2] = 2; matrix2[0][3] = 3;
matrix2[1][0] = 0; matrix2[1][1] = 1; matrix2[1][2] = 2; matrix2[1][3] = 3;
matrix2[2][0] = 0; matrix2[2][1] = 1; matrix2[2][2] = 2; matrix2[2][3] = 3;
matrix2[3][0] = 0; matrix2[3][1] = 1; matrix2[3][2] = 2; matrix2[3][3] = 3;
Utils::printMatrix("Matrix 1", matrix1);
Utils::printMatrix("Matrix 2", matrix2);
cout << "After initialization" << endl;
// Algorithm
vector<int> result = algGaleShapley(matrix1, matrix2);
cout << "After algorithm" << endl;
// Print results
cout << "Results: " << endl;
for (int i = 0; i < result.size(); i++) {
cout << result[i] << ", ";
}
}
void dgWorld::BodySetMatrix (dgBody* const body, const dgMatrix& matrix)
{
#define DG_RECURSIVE_SIZE 1024
dgBody* queue[DG_RECURSIVE_SIZE];
dgInt32 index = 1;
queue[0] = body;
m_genericLRUMark ++;
body->m_genericLRUMark = m_genericLRUMark;
dgMatrix relMatrix (body->GetMatrix().Inverse() * matrix);
while (index) {
index --;
dgBody* body1 = queue[index];
dgAssert (body1 != m_sentinelBody);
// why should I do this? I do no remember the reason
//m_broadPhase->Remove (body);
//m_broadPhase->Add (body);
dgMatrix matrix1 (body1->GetMatrix() * relMatrix);
//body1->SetOmega (dgVector (dgFloat32 (0.0f)));
//body1->SetVelocity (dgVector (dgFloat32 (0.0f)));
body1->SetOmega (matrix1.RotateVector (body1->GetOmega()));
body1->SetVelocity (matrix1.RotateVector (body1->GetVelocity()));
body1->SetMatrix (matrix1);
body1->UpdateCollisionMatrix (dgFloat32 (0.0f), 0);
body1->SetSleepState(false);
for (dgBodyMasterListRow::dgListNode* jointNode = body1->m_masterNode->GetInfo().GetFirst(); jointNode; jointNode = jointNode->GetNext()) {
dgBodyMasterListCell& cell = jointNode->GetInfo();
body1 = cell.m_bodyNode;
if (body1 != m_sentinelBody) {
if (body1->m_genericLRUMark != m_genericLRUMark) {
dgConstraint* constraint;
constraint = cell.m_joint;
if (constraint->GetId() != dgConstraint::m_contactConstraint) {
body1->m_genericLRUMark = m_genericLRUMark;
queue[index] = body1;
index ++;
dgAssert (index < DG_RECURSIVE_SIZE);
}
}
}
}
}
}
void CustomJoint::CalculateLocalMatrix (const dMatrix& pinsAndPivotFrame, dMatrix& localMatrix0, dMatrix& localMatrix1) const
{
dMatrix matrix0;
// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
dMatrix matrix1 (dGetIdentityMatrix());
if (m_body1) {
NewtonBodyGetMatrix(m_body1, &matrix1[0][0]);
}
// create a global matrix at the pivot point with front vector aligned to the pin vector
dAssert (pinsAndPivotFrame.SanityCheck());
// calculate the relative matrix of the pin and pivot on each body
localMatrix0 = pinsAndPivotFrame * matrix0.Inverse();
localMatrix1 = pinsAndPivotFrame * matrix1.Inverse();
}
void NewtonCustomJoint::CalculateLocalMatrix (const dVector& pivot, const dVector& dir, dMatrix& localMatrix0, dMatrix& localMatrix1) const
{
dMatrix matrix0;
// Get the global matrices of each rigid body.
NewtonBodyGetMatrix(m_body0, &matrix0[0][0]);
dMatrix matrix1 (GetIdentityMatrix());
if (m_body1) {
NewtonBodyGetMatrix(m_body1, &matrix1[0][0]);
}
// create a global matrix at the pivot point with front vector aligned to the pin vector
dMatrix pinAndPivotMatrix (dgGrammSchmidt(dir));
pinAndPivotMatrix.m_posit = pivot;
pinAndPivotMatrix.m_posit.m_w = 1.0f;
// calculate the relative matrix of the pin and pivot on each body
localMatrix0 = pinAndPivotMatrix * matrix0.Inverse();
localMatrix1 = pinAndPivotMatrix * matrix1.Inverse();
}
GAIndividual<CodeVInt> GSTM::crossover(const GAIndividual<CodeVInt> &indivl1, const GAIndividual<CodeVInt> &indivl2)
{
if (indivl1 == indivl2)
return indivl1;
GAIndividual<CodeVInt> indivl;
int i, j, n;
vector<int> arr(m_numDim), flag(m_numDim+1);
vector<vector<int>> matrix1(m_numDim), matrix2(m_numDim);
for (i = 0; i < m_numDim; i++)
{
matrix1[i].resize(m_numDim);
matrix2[i].resize(m_numDim);
}
map<int, vector<int> > frag;
for (i = 0; i<m_numDim; i++)
arr[i] = indivl1.data().m_x[i];
for (i = 0; i<m_numDim + 1; i++)
flag[i] = 0;
for (i = 0; i<m_numDim; i++)
{
for (j = 0; j<m_numDim; j++)
{
matrix1[i][j] = 0;
matrix2[i][j] = 0;
}
}
for (i = 0; i<m_numDim; i++)
{
matrix1[arr[i]][arr[(i + 1) % m_numDim]] = 1;
matrix1[arr[(i + 1) % m_numDim]][arr[i]] = 1;
matrix2[indivl2.data().m_x[i]][indivl2.data().m_x[(i + 1) % m_numDim]] = 1;
matrix2[indivl2.data().m_x[(i + 1) % m_numDim]][indivl2.data().m_x[i]] = 1;
}
for (i = 0; i<m_numDim; i++)
{
if (matrix2[arr[i]][arr[(i + 1) % m_numDim]] == 0)
flag[i + 1] = 1;
}
flag[0] = flag[m_numDim];
int m = 0;
n = 0;
j = 0;
frag[n].push_back(arr[j++]);
m++;
while (flag[j] == 0 && m<m_numDim)
{
frag[n].push_back(arr[j++]);
m++;
}
i = m_numDim;
while (flag[i] == 0 && m<m_numDim)
{
frag[n].insert(frag[n].begin(), arr[i - 1]);
i--;
m++;
}
while (m<m_numDim)
{
if (flag[j] == 1) n++;
frag[n].push_back(arr[j++]);
m++;
while (flag[j] == 0)
{
frag[n].push_back(arr[j++]);
m++;
}
}
vector<int> visited(m_numDim);
map<int, int> mp;
for (i = 0; i<m_numDim; i++)
visited[i] = 0;
m = 0;
for (i = 0; i <= n; i++)
{
visited[m++] = frag[i].front();
mp[visited[m - 1]] = i;
if (frag[i].size()>1)
{
visited[m++] = frag[i].back();
mp[visited[m - 1]] = i;
}
}
int starts;
i = Global::msp_global->getRandInt(0, m - 1);
j = 0;
arr[j++] = visited[i];
starts = visited[i];
if (frag[mp[visited[i]]].front() == starts)
{
for (size_t z = 1; z<frag[mp[visited[i]]].size(); z++)
arr[j++] = frag[mp[visited[i]]][z];
starts = frag[mp[visited[i]]].back();
if (frag[mp[visited[i]]].size()>1)
{
int fg = 0;
if (visited[m - 1] == visited[i]) fg = 1;
for (int z = 0; z<m; z++)
{
if (visited[z] == frag[mp[visited[i]]].back())
{
visited[z] = visited[m - 1];
m--;
if (fg == 1) i = z;
break;
}
}
}
visited[i] = visited[m - 1];
m--;
}
else if (frag[mp[visited[i]]].back() == starts)
{
for (int z = frag[mp[visited[i]]].size() - 2; z >= 0; z--)
arr[j++] = frag[mp[visited[i]]][z];
starts = frag[mp[visited[i]]].front();
if (frag[mp[visited[i]]].size()>1)
{
int fg = 0;
if (visited[m - 1] == visited[i]) fg = 1;
for (int z = 0; z<m; z++)
{
if (visited[z] == frag[mp[visited[i]]].front())
{
visited[z] = visited[m - 1];
m--;
if (fg == 1) i = z;
break;
}
}
}
visited[i] = visited[m - 1];
m--;
}
n--;
for (i = 0; i <= n; i++)
{
double min = DBL_MAX;
int temp;
int pos = -1;
TravellingSalesman * ptr = dynamic_cast<TravellingSalesman*>(Global::msp_global->mp_problem.get());
for (int z = 0; z<m; z++)
{
if (min>ptr->getCost()[starts][visited[z]] && matrix1[starts][visited[z]] == 0 && matrix2[starts][visited[z]] == 0)
{
min = ptr->getCost()[starts][visited[z]];
temp = visited[z];
pos = z;
}
}
if (pos == -1)
{
temp = visited[0];
pos = 0;
}
arr[j++] = temp;
if (frag[mp[temp]].front() == temp)
{
for (size_t z = 1; z<frag[mp[temp]].size(); z++)
arr[j++] = frag[mp[temp]][z];
starts = frag[mp[temp]].back();
if (frag[mp[temp]].size()>1)
{
int fg = 0;
if (visited[m - 1] == visited[pos]) fg = 1;
for (int z = 0; z<m; z++)
{
if (visited[z] == frag[mp[temp]].back())
{
visited[z] = visited[m - 1];
m--;
if (fg == 1) pos = z;
break;
}
}
}
visited[pos] = visited[m - 1];
m--;
}
else if (frag[mp[temp]].back() == temp)
{
for (int z = frag[mp[temp]].size() - 2; z >= 0; z--)
arr[j++] = frag[mp[temp]][z];
starts = frag[mp[temp]].front();
if (frag[mp[temp]].size()>1)
{
int fg = 0;
if (visited[m - 1] == visited[pos]) fg = 1;
for (int z = 0; z<m; z++)
{
if (visited[z] == frag[mp[temp]].front())
{
visited[z] = visited[m - 1];
m--;
if (fg == 1) pos = z;
break;
}
}
}
visited[pos] = visited[m - 1];
m--;
}
}
for (i = 0; i<m_numDim; i++)
indivl.data().m_x[i] = arr[i];
return indivl;
}
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();
}
gmx_bool mrqmin_new(real x[],real y[],real sig[],int ndata,real a[],
int ia[],int ma,real **covar,real **alpha,real *chisq,
void (*funcs)(real, real [], real *, real []),
real *alamda)
/* Levenberg-Marquardt method, attempting to reduce the value Chi^2
* of a fit between a set of data points x[1..ndata], y[1..ndata]
* with individual standard deviations sig[1..ndata], and a nonlinear
* function dependent on ma coefficients a[1..ma]. The input array
* ia[1..ma] indicates by nonzero entries those components of a that
* should be fitted for, and by zero entries those components that
* should be held fixed at their input values. The program returns
* current best-fit values for the parameters a[1..ma], and
* Chi^2 = chisq. The arrays covar[1..ma][1..ma], alpha[1..ma][1..ma]
* are used as working space during most iterations. Supply a routine
* funcs(x,a,yfit,dyda,ma) that evaluates the fitting function yfit,
* and its derivatives dyda[1..ma] with respect to the fitting
* parameters a at x. On the first call provide an initial guess for
* the parameters a, and set alamda < 0 for initialization (which then
* sets alamda=.001). If a step succeeds chisq becomes smaller and
* alamda de-creases by a factor of 10. If a step fails alamda grows by
* a factor of 10. You must call this routine repeatedly until
* convergence is achieved. Then, make one final call with alamda=0,
* so that covar[1..ma][1..ma] returns the covariance matrix, and alpha
* the curvature matrix.
* (Parameters held fixed will return zero covariances.)
*/
{
void covsrt(real **covar, int ma, int ia[], int mfit);
gmx_bool gaussj(real **a, int n, real **b,int m);
void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
int ia[], int ma, real **alpha, real beta[], real *chisq,
void (*funcs)(real, real [], real *, real []));
int j,k,l;
static int mfit;
static real ochisq,*atry,*beta,*da,**oneda;
if (*alamda < 0.0) { /* Initialization. */
atry=rvector(1,ma);
beta=rvector(1,ma);
da=rvector(1,ma);
for (mfit=0,j=1;j<=ma;j++)
if (ia[j]) mfit++;
oneda=matrix1(1,mfit,1,1);
*alamda=0.001;
mrqcof_new(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
ochisq=(*chisq);
for (j=1;j<=ma;j++)
atry[j]=a[j];
}
for (j=1;j<=mfit;j++) { /* Alter linearized fitting matrix, by augmenting. */
for (k=1;k<=mfit;k++)
covar[j][k]=alpha[j][k]; /* diagonal elements. */
covar[j][j]=alpha[j][j]*(1.0+(*alamda));
oneda[j][1]=beta[j];
}
if (!gaussj(covar,mfit,oneda,1)) /* Matrix solution. */
return FALSE;
for (j=1;j<=mfit;j++)
da[j]=oneda[j][1];
if (*alamda == 0.0) { /* Once converged, evaluate covariance matrix. */
covsrt_new(covar,ma,ia,mfit);
free_matrix(oneda,1,mfit,1);
free_vector(da,1);
free_vector(beta,1);
free_vector(atry,1);
return TRUE;
}
for (j=0,l=1;l<=ma;l++) /* Did the trial succeed? */
if (ia[l]) atry[l]=a[l]+da[++j];
mrqcof_new(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
if (*chisq < ochisq) {
/* Success, accept the new solution. */
*alamda *= 0.1;
ochisq=(*chisq);
for (j=1;j<=mfit;j++) {
for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
beta[j]=da[j];
}
for (l=1;l<=ma;l++) a[l]=atry[l];
} else { /* Failure, increase alamda and return. */
*alamda *= 10.0;
*chisq=ochisq;
}
return TRUE;
}
int tests() {
{
//assignment
Matrix<double> matrix(5, 5, 1);
Matrix<double> copyMatrix = matrix;
if (matrix != copyMatrix) {
return 101;
}
}
{
//test simple matrix accessor
Matrix<double> matrix(5, 5, 1);
if (matrix[1][1] != 1) {
return 102;
}
if (matrix(1, 1) != 1) {
return 103;
}
}
{
//test matrix addition
Matrix<double> matrix1(3, 3, 1);
Matrix<double> matrix2(3, 3, 3);
Matrix<double> result(3, 3, 4);
Matrix<double> matrix3 = matrix1 + matrix2;
if (matrix3 != result) {
return 104;
}
}
{
//test cumulative matrix addition
Matrix<double> matrix1(3, 3, 1);
Matrix<double> matrix2(3, 3, 3);
Matrix<double> result(3, 3, 4);
matrix1 += matrix2;
if (matrix1 != result) {
return 105;
}
}
{
//test matrix subtraction
Matrix<double> matrix1(5, 5, 20);
Matrix<double> matrix2(5, 5, 50);
Matrix<double> result(5, 5, -30);
Matrix<double> matrix3 = matrix1 - matrix2;
if (matrix3 != result) {
return 106;
}
}
{
//test cumulative matrix subtraction
Matrix<double> matrix1(5, 5, 20);
Matrix<double> matrix2(5, 5, 50);
Matrix<double> result(5, 5, -30);
matrix1 -= matrix2;
if (matrix1 != result) {
return 107;
}
}
{
//test matrix multiplication
Matrix<double> matrix1(5, 3, 1);
Matrix<double> matrix2(5, 5, 2);
Matrix<double> matrix3 = matrix1 * matrix2;
Matrix<double> result(5, 3, 15);
if (result != matrix3) {
return 108;
}
}
{
//test cumulative multiplication
Matrix<double> matrix1(5, 3, 1);
Matrix<double> matrix2(5, 5, 2);
matrix1 *= matrix2;
Matrix<double> result(5, 3, 15);
if (result != matrix1) {
return 109;
}
}
{
Matrix<string> matrix("sorin");
Matrix<string> result(1, 1, "sorin");
if (result != matrix) {
return 110;
}
}
{
Matrix<double> matrix1(5, 5, 6);
Matrix<double> matrix2(matrix1);
if (matrix1 != matrix2) {
return 111;
}
}
return 0;
}
static void AddPathFollow (DemoEntityManager* const scene, const dVector& origin)
{
// create a Bezier Spline path for AI car to drive
DemoEntity* const rollerCosterPath = new DemoEntity(dGetIdentityMatrix(), NULL);
scene->Append(rollerCosterPath);
dBezierSpline spline;
dFloat64 knots[] = {0.0f, 1.0f / 5.0f, 2.0f / 5.0f, 3.0f / 5.0f, 4.0f / 5.0f, 1.0f};
dBigVector o (origin[0], origin[1], origin[2], 0.0f);
dBigVector control[] =
{
dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 10.0f, 150.0f - 250.0f, 1.0f) + o,
dBigVector(175.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(200.0f - 100.0f, 70.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(215.0f - 100.0f, 20.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(150.0f - 100.0f, 50.0f, 350.0f - 250.0f, 1.0f) + o,
dBigVector( 50.0f - 100.0f, 30.0f, 250.0f - 250.0f, 1.0f) + o,
dBigVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 1.0f) + o,
};
spline.CreateFromKnotVectorAndControlPoints(3, sizeof (knots) / sizeof (knots[0]), knots, control);
DemoBezierCurve* const mesh = new DemoBezierCurve (spline);
rollerCosterPath->SetMesh(mesh, dGetIdentityMatrix());
mesh->SetVisible(true);
mesh->SetRenderResolution(500);
mesh->Release();
const int count = 32;
NewtonBody* bodies[count];
dBigVector point0;
dVector positions[count + 1];
dFloat64 knot = spline.FindClosestKnot(point0, origin + dVector(100.0f - 100.0f, 20.0f, 200.0f - 250.0f, 0.0f), 4);
positions[0] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0);
dFloat average = 0.0f;
for (int i = 0; i < count; i ++) {
dBigVector point1;
average += positions[i].m_y;
dBigVector tangent(spline.CurveDerivative(knot));
tangent = tangent.Scale (1.0 / sqrt (tangent % tangent));
knot = spline.FindClosestKnot(point1, dBigVector (point0 + tangent.Scale (2.0f)), 4);
point0 = point1;
positions[i + 1] = dVector (point0.m_x, point0.m_y, point0.m_z, 0.0);
}
average /= count;
for (int i = 0; i < count + 1; i ++) {
positions[i].m_y = average;
}
dFloat attachmentOffset = 0.8f;
for (int i = 0; i < count; i ++) {
dMatrix matrix;
dVector location0 (positions[i].m_x, positions[i].m_y, positions[i].m_z, 0.0);
bodies[i] = CreateWheel(scene, location0, 1.0f, 0.5f);
NewtonBodySetLinearDamping(bodies[i], 0.0f);
NewtonBody* const box = bodies[i];
NewtonBodyGetMatrix(box, &matrix[0][0]);
dVector location1 (positions[i + 1].m_x, positions[i + 1].m_y, positions[i + 1].m_z, 0.0);
dVector dir (location1 - location0);
matrix.m_front = dir.Scale (1.0f / dSqrt (dir % dir));
matrix.m_right = matrix.m_front * matrix.m_up;
dMatrix matrix1 (dYawMatrix(0.5f * 3.141692f) * matrix);
NewtonBodySetMatrix(box, &matrix1[0][0]);
matrix.m_posit.m_y += attachmentOffset;
new MyPathFollow(matrix, box, rollerCosterPath);
dVector veloc (dir.Scale (20.0f));
NewtonBodySetVelocity(box, &veloc[0]);
}
for (int i = 1; i < count; i ++) {
NewtonBody* const box0 = bodies[i - 1];
NewtonBody* const box1 = bodies[i];
dMatrix matrix0;
dMatrix matrix1;
NewtonBodyGetMatrix(box0, &matrix0[0][0]);
NewtonBodyGetMatrix(box1, &matrix1[0][0]);
matrix0.m_posit.m_y += attachmentOffset;
matrix1.m_posit.m_y += attachmentOffset;
new CustomDistanceRope (matrix1.m_posit, matrix0.m_posit, box1, box0);
}
void* const aggregate = NewtonCollisionAggregateCreate (scene->GetNewton());
for (int i = 0; i < count; i ++) {
NewtonCollisionAggregateAddBody(aggregate, bodies[i]);
}
NewtonCollisionAggregateSetSelfCollision (aggregate, false);
#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate(scene->GetNewton(), bodies[0], NULL);
for (int i = 1; i < count; i++) {
NewtonSkeletonContainerAttachBone(skeleton, bodies[i], bodies[i - 1]);
}
NewtonSkeletonContainerFinalize(skeleton);
#endif
}
void testMatrix3()
{
reportTesting("Matrix3 class");
Matrix3 matrix3d = Matrix3(
1.0f, 2.0f, 3.0f,
4.0f, 5.0f, 6.0f,
7.0f, 8.0f, 9.0f);
assertEquals(__LINE__,
1.0f, 2.0f, 3.0f,
4.0f, 5.0f, 6.0f,
7.0f, 8.0f, 9.0f, matrix3d);
//Constructors
Matrix3 matrix = Matrix3(matrix3d);
assertEquals(__LINE__, matrix, matrix3d);
matrix3d = Matrix3();
assertEquals(__LINE__,
0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, matrix3d);
assertEquals(__LINE__,
1.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 1.0f,
Matrix3::newIdentity());
//Getters and setters
matrix3d.set(
9.0f, 8.0f, 7.0f,
6.0f, 5.0f, 4.0f,
3.0f, 2.0f, 1.0f);
assertEquals(__LINE__,
9.0f, 8.0f, 7.0f,
6.0f, 5.0f, 4.0f,
3.0f, 2.0f, 1.0f, matrix3d);
matrix3d(0,0) = 10.0f;
matrix3d(0,1) = 11.0f;
matrix3d(0,2) = 12.0f;
matrix3d(1,0) = 13.0f;
matrix3d(1,1) = 14.0f;
matrix3d(1,2) = 15.0f;
matrix3d(2,0) = 16.0f;
matrix3d(2,1) = 17.0f;
matrix3d(2,2) = 18.0f;
assertEquals(__LINE__,
10.0f, 11.0f, 12.0f,
13.0f, 14.0f, 15.0f,
16.0f, 17.0f, 18.0f, matrix3d);
matrix3d = matrix1();
const Matrix3 m = matrix1();
assertEquals(__LINE__, 1.0f, m(0,0));
assertEquals(__LINE__, 2.0f, m(0,1));
assertEquals(__LINE__, 3.0f, m(0,2));
assertEquals(__LINE__, 4.0f, m(0,3));
assertEquals(__LINE__, 5.0f, m(0,4));
assertEquals(__LINE__, 6.0f, m(0,5));
assertEquals(__LINE__, 7.0f, m(0,6));
assertEquals(__LINE__, 8.0f, m(0,7));
assertEquals(__LINE__, 9.0f, m(0,8));
//Operators
matrix3d = matrix1();
matrix3d += matrix2();
assertEquals(__LINE__,
2.0f, 11.0f, 5.0f,
12.0f, 8.0f, 13.0f,
11.0f, 14.0f, 14.0f, matrix3d);
matrix3d = matrix1();
Matrix3 other = matrix3d + matrix2();
assertEquals(__LINE__, matrix1(), matrix3d);
assertEquals(__LINE__,
2.0f, 11.0f, 5.0f,
12.0f, 8.0f, 13.0f,
11.0f, 14.0f, 14.0f, other);
matrix3d = matrix1();
matrix3d -= matrix2();
assertEquals(__LINE__,
0.0f, -7.0f, 1.0f,
-4.0f, 2.0f, -1.0f,
3.0f, 2.0f, 4.0f, matrix3d);
matrix3d = matrix1();
other = matrix3d - matrix2();
assertEquals(__LINE__, matrix1(), matrix3d);
assertEquals(__LINE__,
0.0f, -7.0f, 1.0f,
-4.0f, 2.0f, -1.0f,
3.0f, 2.0f, 4.0f, other);
matrix3d = matrix1();
matrix3d *= Matrix3(
9.0f, 8.0f, 7.0f,
6.0f, 10.0f, 4.0f,
3.0f, 2.0f, 1.0f);
assertEquals(__LINE__,
30.0f, 34.0f, 18.0f,
84.0f, 94.0f, 54.0f,
138.0f, 154.0f, 90.0f, matrix3d);
matrix3d = matrix1();
other = matrix3d * Matrix3(
9.0f, 8.0f, 7.0f,
6.0f, 10.0f, 4.0f,
3.0f, 2.0f, 1.0f);
assertEquals(__LINE__, matrix1(), matrix3d);
assertEquals(__LINE__,
30.0f, 34.0f, 18.0f,
84.0f, 94.0f, 54.0f,
138.0f, 154.0f, 90.0f, other);
matrix3d = matrix1();
matrix3d *= 3;
assertEquals(__LINE__,
3.0f, 6.0f, 9.0f,
12.0f, 15.0f, 18.0f,
21.0f, 24.0f, 27.0f, matrix3d);
matrix3d = matrix1();
other = matrix1() * 3;
assertEquals(__LINE__,
3.0f, 6.0f, 9.0f,
12.0f, 15.0f, 18.0f,
21.0f, 24.0f, 27.0f, other);
matrix3d = matrix1();
other = 2 * matrix1();
assertEquals(__LINE__,
2.0f, 4.0f, 6.0f,
8.0f, 10.0f, 12.0f,
14.0f, 16.0f, 18.0f, other);
//Methods
matrix3d.set(
5, 0, 1,
-2, 3, 4,
0, 2, -1);
assertEquals(__LINE__, -59.0f, matrix3d.determinant());
assertTrue(__LINE__, matrix3d.isInvertible());
matrix3d.set(
1, 3, 10,
-1, 1, 10,
0, 2, 10);
assertEquals(__LINE__, 0.0f, matrix3d.determinant());
assertTrue(__LINE__, !matrix3d.isInvertible());
matrix3d.set(
3, 0, 2,
9, 1, 7,
1, 0, 1);
matrix3d.inverse();
assertEquals(__LINE__,
1.0f, 0.0f, -2.0f,
-2.0f, 1.0f, -3.0f,
-1.0f, 0.0f, 3.0f, matrix3d);
matrix3d = matrix1();
matrix3d.transpose();
assertEquals(__LINE__,
1.0f, 4.0f, 7.0f,
2.0f, 5.0f, 8.0f,
3.0f, 6.0f, 9.0f, matrix3d);
//Auxiliary functions
matrix3d = matrix1();
other = transpose(matrix3d);
assertEquals(__LINE__, matrix1(), matrix3d);
assertEquals(__LINE__,
1.0f, 4.0f, 7.0f,
2.0f, 5.0f, 8.0f,
3.0f, 6.0f, 9.0f, other);
matrix3d.set(
3, 0, 2,
9, 1, 7,
1, 0, 1);
other = inverse(matrix3d);
assertEquals(__LINE__,
3, 0, 2,
9, 1, 7,
1, 0, 1, matrix3d);
assertEquals(__LINE__,
1.0f, 0.0f, -2.0f,
-2.0f, 1.0f, -3.0f,
-1.0f, 0.0f, 3.0f, other);
}
gmx_bool mrqmin(real x[], real y[], real sig[], int ndata, real a[],
int ma, int lista[], int mfit,
real **covar, real **alpha, real *chisq,
void (*funcs)(real,real *,real *,real *),
real *alamda)
{
int k,kk,j,ihit;
static real *da,*atry,**oneda,*beta,ochisq;
if (*alamda < 0.0) {
oneda=matrix1(1,mfit,1,1);
atry=rvector(1,ma);
da=rvector(1,ma);
beta=rvector(1,ma);
kk=mfit+1;
for (j=1;j<=ma;j++) {
ihit=0;
for (k=1;k<=mfit;k++)
if (lista[k] == j) ihit++;
if (ihit == 0)
lista[kk++]=j;
else if (ihit > 1) {
nrerror("Bad LISTA permutation in MRQMIN-1", FALSE);
return FALSE;
}
}
if (kk != ma+1) {
nrerror("Bad LISTA permutation in MRQMIN-2", FALSE);
return FALSE;
}
*alamda=0.001;
mrqcof(x,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs);
ochisq=(*chisq);
}
for (j=1;j<=mfit;j++) {
for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
covar[j][j]=alpha[j][j]*(1.0+(*alamda));
oneda[j][1]=beta[j];
}
if (!gaussj(covar,mfit,oneda,1))
return FALSE;
for (j=1;j<=mfit;j++)
da[j]=oneda[j][1];
if (*alamda == 0.0) {
covsrt(covar,ma,lista,mfit);
free_vector(beta,1);
free_vector(da,1);
free_vector(atry,1);
free_matrix(oneda,1,mfit,1);
return TRUE;
}
for (j=1;j<=ma;j++) atry[j]=a[j];
for (j=1;j<=mfit;j++)
atry[lista[j]] = a[lista[j]]+da[j];
mrqcof(x,y,sig,ndata,atry,ma,lista,mfit,covar,da,chisq,funcs);
if (*chisq < ochisq) {
*alamda *= 0.1;
ochisq=(*chisq);
for (j=1;j<=mfit;j++) {
for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
beta[j]=da[j];
a[lista[j]]=atry[lista[j]];
}
} else {
*alamda *= 10.0;
*chisq=ochisq;
}
return TRUE;
}