///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2009, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// * Neither the name of Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////
#include <testMiscMatrixAlgo.h>
#include "ImathMatrixAlgo.h"
#include "ImathRandom.h"
#include <iostream>
#include <exception>
#include <stdio.h>
#include <assert.h>
#if 0
#define debug(x) (printf x, fflush (stdout))
#else
#define debug(x)
#endif
using namespace std;
using namespace IMATH_INTERNAL_NAMESPACE;
namespace {
float rad (float deg) {return deg * (M_PI / 180);}
void
testComputeLocalFrame ()
{
float eps = 0.00005;
Rand48 random(0);
for (int i = 0; i < 100000; ++i)
{
debug (("iteration: %d\n", i));
// Random pos
V3f p (random.nextf (-10, 10),
random.nextf (-10, 10),
random.nextf (-10, 10));
// Random xDir
V3f xDir (random.nextf (-10, 10),
random.nextf (-10, 10),
random.nextf (-10, 10));
// Random normalDir
V3f normalDir (random.nextf (-10, 10),
random.nextf (-10, 10),
random.nextf (-10, 10));
// Run computeLocalFrame we want to test
M44f L = computeLocalFrame(p, xDir, normalDir);
// test position
for (int j=0; j<3; j++)
{
if ( abs(L[3][j] - p[j])>eps )
assert(false);
}
if (abs (L[3][3] - 1.0)>eps)
assert(false );
// check that xAxis has the same dir as xDir and that is is normalized
V3f x( L[0][0], L[0][1], L[0][2]);
assert( (x%xDir).length() < eps );
if (abs (L[0][3])>eps)
assert(false);
assert((abs(x.length()-1.f)<eps));
// Check that y is normal to x and to normalDir, and is normalized
V3f y( L[1][0], L[1][1], L[1][2]);
if (abs (L[1][3])>eps)
assert(false );
assert(abs(x^y)<eps);
/*std::cout<<y<<"\n";
std::cout<<normalDir<<"\n";
std::cout<<(y^normalDir)<<"\n";*/
assert(abs(y^normalDir)<eps);
assert((abs(y.length()-1.f)<eps));
// check that z is normalized, normal to x and y, and direct
V3f z( L[2][0], L[2][1], L[2][2]);
if (abs (L[2][3])>eps)
assert(false );
assert((abs(z.length()-1.f)<eps));
assert(abs(x^z)<eps);
assert(abs(y^z)<eps);
assert(((x%y)^z)>0);
}
}
void
getRandTRS(Rand48& random, V3f& trans, V3f& rot, V3f& scale)
{
// Translate
trans = V3f (random.nextf (-10, 10),
random.nextf (-10, 10),
random.nextf (-10, 10));
// Rotate
rot = V3f (rad (random.nextf (-180, 180)),
rad (random.nextf (-180, 180)),
rad (random.nextf (-180, 180)));
// Scale
V3f s(random.nextf (0.000001, 2.0),
random.nextf (0.000001, 2.0),
random.nextf (0.000001, 2.0));
for (int j=0; j < 3; j++)
if (random.nextf (0.0, 1.0) >= 0.5)
s[j] *= -1;
scale = s;
}
M44f
createRandomMat(Rand48& random, V3f& trans, V3f& rot, V3f& scale)
{
M44f M;
V3f t, r, s;
getRandTRS(random, t, r, s);
M.translate (t);
M.rotate (r);
// Shear M.
V3f h (random.nextf (0.000001, 2.0),
random.nextf (0.000001, 2.0),
random.nextf (0.000001, 2.0));
for (int j=0; j < 3; j++)
if (random.nextf (0.0, 1.0) >= 0.5)
h[j] *= -1;
M.shear (h);
M.scale (s);
//
// Add a small random error to the elements of M
//
for (int j = 0; j < 4; ++j)
for (int k = 0; k < 3; ++k)
M[j][k] += random.nextf (-1e-7, 1e-7);
V3f sh;
extractSHRT (M, scale, sh, rot, trans);
debug (("Scale : %f %f %f\n", s[0], s[1], s[2]));
debug (("Shear : %f %f %f\n", h[0], h[1], h[2]));
debug (("Rot : %f %f %f\n", r[0], r[1], r[2]));
debug (("Trans : %f %f %f\n", t[0], t[1], t[2]));
return M;
}
void
compareMat(M44f& M, M44f& N)
{
float eps = 0.0001;
/// Verify that the entries in M and N do not
// differ too much.
M44f D (M - N);
for (int j = 0; j < 4; ++j)
{
for (int k = 0; k < 4; ++k)
{
//cout << "diff="<<D[j][k] << endl;
if (abs (D[j][k]) > eps)
{
cout << "unexpectedly diff "<<
D[j][k] << endl;
cout << j << " " << k << endl;
cout << "M\n" << M << endl;
cout << "N\n" << N << endl;
cout << "D\n" << D << endl;
assert (false);
}
}
}
}
void
testAddOffset()
{
Rand48 random(0);
for (int i = 0; i < 100000; ++i)
{
debug (("iteration: %d\n", i));
V3f transA, transB, rotA, rotB, scaleA, scaleB;
V3f tOffset, rOffset, sOffset;
M44f inMat = createRandomMat(random, transA, rotA, scaleA);
M44f refMat = createRandomMat(random, transB, rotB, scaleB);
getRandTRS(random, tOffset, rOffset, sOffset);
// addOffset : function to test
M44f outMat = addOffset( inMat, tOffset, rOffset, sOffset, refMat);
// add the inverse offset
M44f invO;
invO.rotate (V3f(rad(rOffset[0]), rad(rOffset[1]), rad(rOffset[2])));
invO[3][0] = tOffset[0];
invO[3][1] = tOffset[1];
invO[3][2] = tOffset[2];
invO.invert();
M44f invS;
invS.scale (sOffset);
invS.invert(); // zero scale is avoided in getRandTRS
// in ref mat from the function result
M44f outInRefMat = invO*invS*outMat;
// in ref mat from the inputs
M44f inRefMat = inMat*refMat;
// compare the mat
compareMat(outInRefMat, inRefMat);
}
}
void
testRSMatrix(M44f& M, V3f& t, V3f& r, V3f& s)
{
M44f N;
N.makeIdentity();
N.translate (t); // ... matrix compositions
N.rotate (r);
N.scale (s);
compareMat(M, N);
}
void
testComputeRSMatrix ()
{
Rand48 random(0);
for (int i = 0; i < 100000; ++i)
{
debug (("iteration: %d\n", i));
V3f transA, transB, rotA, rotB, scaleA, scaleB;
M44f A = createRandomMat(random, transA, rotA, scaleA);
M44f B = createRandomMat(random, transB, rotB, scaleB);
M44f ArAsA = computeRSMatrix( true, true, A, B);
M44f ArBsB = computeRSMatrix( false, false, A, B);
M44f ArAsB = computeRSMatrix( true, false, A, B);
M44f ArBsA = computeRSMatrix( false, true, A, B);
testRSMatrix(ArAsA, transA, rotA, scaleA);
testRSMatrix(ArBsB, transA, rotB, scaleB);
testRSMatrix(ArAsB, transA, rotA, scaleB);
testRSMatrix(ArBsA, transA, rotB, scaleA);
debug (("\n"));
}
}
} // namespace
void
testMiscMatrixAlgo ()
{
try
{
cout << "Testing misc functions in ImathMatrixAlgo.h" << endl;
cout << "Testing the building of an orthonormal direct frame from : a position, "
<< "an x axis direction and a normal to the y axis" << endl;
cout << "IMATH_INTERNAL_NAMESPACE::computeLocalFrame()" << endl;
testComputeLocalFrame ();
cout << "ok\n" << endl;
cout << "Add a translate/rotate/scale offset to an input frame "
<< "and put it in another frame of reference" << endl;
cout << "IMATH_INTERNAL_NAMESPACE::addOffset()" << endl;
testAddOffset ();
cout << "ok\n" << endl;
cout << "Compute Translate/Rotate/Scale matrix from matrix A "<<endl;
cout << "with the Rotate/Scale of Matrix B"<< endl;
cout << "IMATH_INTERNAL_NAMESPACE::computeRSMatrix()" << endl;
testComputeRSMatrix ();
cout << "ok\n" << endl;
}
catch (std::exception &e)
{
cerr << " Caught exception: " << e.what () << endl;
}
}