342 lines
9.5 KiB
C++
342 lines
9.5 KiB
C++
|
///////////////////////////////////////////////////////////////////////////
|
||
|
|
//
|
||
|
|
// 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;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|