Example #1
0
void draw_string_into (Imath::M44d m, char* string, std::vector< std::vector<vec_t<2> > >* polygons) {
  Imath::M44d nm = m;
  for (int i = 0; i < strlen(string); i++) {
    draw_letter_into(nm, string[i], polygons);
    nm.translate(Imath::V3d(1, 0, 0));
  }
}
void
testProcrustesImp ()
{
    // Test the empty case:
    Imath::M44d id = 
        procrustesRotationAndTranslation ((Imath::Vec3<T>*) 0, 
                                          (Imath::Vec3<T>*) 0,
                                          (T*) 0,
                                          0);
    assert (id == Imath::M44d());

    id = procrustesRotationAndTranslation ((Imath::Vec3<T>*) 0, 
                                           (Imath::Vec3<T>*) 0,
                                           0);
    assert (id == Imath::M44d());

    // First we'll test with a bunch of known translation/rotation matrices
    // to make sure we get back exactly the same points:
    Imath::M44d m;
    m.makeIdentity();
    testTranslationRotationMatrix<T> (m);

    m.translate (Imath::V3d(3.0, 5.0, -0.2));
    testTranslationRotationMatrix<T> (m);

    m.rotate (Imath::V3d(M_PI, 0, 0));
    testTranslationRotationMatrix<T> (m);
    
    m.rotate (Imath::V3d(0, M_PI/4.0, 0));
    testTranslationRotationMatrix<T> (m);

    m.rotate (Imath::V3d(0, 0, -3.0/4.0 * M_PI));
    testTranslationRotationMatrix<T> (m);

    m.makeIdentity();
    testWithTranslateRotateAndScale<T> (m);

    m.translate (Imath::V3d(0.4, 6.0, 10.0));
    testWithTranslateRotateAndScale<T> (m);

    m.rotate (Imath::V3d(M_PI, 0, 0));
    testWithTranslateRotateAndScale<T> (m);

    m.rotate (Imath::V3d(0, M_PI/4.0, 0));
    testWithTranslateRotateAndScale<T> (m);

    m.rotate (Imath::V3d(0, 0, -3.0/4.0 * M_PI));
    testWithTranslateRotateAndScale<T> (m);

    m.scale (Imath::V3d(2.0, 2.0, 2.0));
    testWithTranslateRotateAndScale<T> (m);

    m.scale (Imath::V3d(0.01, 0.01, 0.01));
    testWithTranslateRotateAndScale<T> (m);

    // Now we'll test with some random point sets and verify
    // the various Procrustes properties:
    std::vector<Imath::Vec3<T> > fromPoints;
    std::vector<Imath::Vec3<T> > toPoints;
    fromPoints.clear(); toPoints.clear();

    for (size_t i = 0; i < 4; ++i)
    {
        const T theta = T(2*i) / T(M_PI);
        fromPoints.push_back (Imath::Vec3<T>(cos(theta), sin(theta), 0));
        toPoints.push_back (Imath::Vec3<T>(cos(theta + M_PI/3.0), sin(theta + M_PI/3.0), 0));
    }
    verifyProcrustes (fromPoints, toPoints);

    Imath::Rand48 random (1209);
    for (size_t numPoints = 1; numPoints < 10; ++numPoints)
    {
        fromPoints.clear(); toPoints.clear();
        for (size_t i = 0; i < numPoints; ++i)
        {
            fromPoints.push_back (Imath::Vec3<T>(random.nextf(), random.nextf(), random.nextf()));
            toPoints.push_back (Imath::Vec3<T>(random.nextf(), random.nextf(), random.nextf()));
        }
    }
    verifyProcrustes (fromPoints, toPoints);

    // Test with some known matrices of varying degrees of quality:
    testProcrustesWithMatrix<T> (m);

    m.translate (Imath::Vec3<T>(3, 4, 1));
    testProcrustesWithMatrix<T> (m);

    m.translate (Imath::Vec3<T>(-10, 2, 1));
    testProcrustesWithMatrix<T> (m);

    Imath::Eulerd rot (M_PI/3.0, 3.0*M_PI/4.0, 0);
    m = m * rot.toMatrix44();
    testProcrustesWithMatrix<T> (m);

    m.scale (Imath::Vec3<T>(1.5, 6.4, 2.0));
    testProcrustesWithMatrix<T> (m);

    Imath::Eulerd rot2 (1.0, M_PI, M_PI/3.0);
    m = m * rot.toMatrix44();

    m.scale (Imath::Vec3<T>(-1, 1, 1));
    testProcrustesWithMatrix<T> (m);

    m.scale (Imath::Vec3<T>(1, 0.001, 1));
    testProcrustesWithMatrix<T> (m);

    m.scale (Imath::Vec3<T>(1, 1, 0));
    testProcrustesWithMatrix<T> (m);
}
void
verifyProcrustes (const std::vector<Imath::Vec3<T> >& from, 
                  const std::vector<Imath::Vec3<T> >& to)
{
    typedef Imath::Vec3<T> V3;

    const T eps = std::sqrt(std::numeric_limits<T>::epsilon());

    const size_t n = from.size();

    // Validate that passing in uniform weights gives the same answer as
    // passing in no weights:
    std::vector<T> weights (from.size());
    for (size_t i = 0; i < weights.size(); ++i)
        weights[i] = 1;
    Imath::M44d m1 = procrustesRotationAndTranslation (&from[0], &to[0], n);
    Imath::M44d m2 = procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n);
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j)
            assert (std::abs(m1[i][j] - m2[i][j]) < eps);

    // Now try the weighted version:
    for (size_t i = 0; i < weights.size(); ++i)
        weights[i] = i+1;

    Imath::M44d m = procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n);

    // with scale:
    Imath::M44d ms = procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n, true);

    // Verify that it's orthonormal w/ positive determinant.
    const T det = m.determinant();
    assert (std::abs(det - T(1)) < eps);

    // Verify orthonormal:
    Imath::M33d upperLeft;
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            upperLeft[i][j] = m[i][j];
    Imath::M33d product = upperLeft * upperLeft.transposed();
    for (int i = 0; i < 3; ++i)
    {
        for (int j = 0; j < 3; ++j)
        {
            const double expected = (i == j ? 1.0 : 0.0);
            assert (std::abs(product[i][j] - expected) < eps);
        }
    }

    // Verify that nearby transforms are worse:
    const size_t numTries = 10;
    Imath::Rand48 rand (1056);
    const double delta = 1e-3;
    for (size_t i = 0; i < numTries; ++i)
    {
        // Construct an orthogonal rotation matrix using Euler angles:
        Imath::Eulerd diffRot (delta * rand.nextf(), delta * rand.nextf(), delta * rand.nextf());
 
        assert (procrustesError (&from[0], &to[0], &weights[0], n, m * diffRot.toMatrix44()) >
                procrustesError (&from[0], &to[0], &weights[0], n, m));

        // Try a small translation:
        Imath::V3d diffTrans (delta * rand.nextf(), delta * rand.nextf(), delta * rand.nextf());
        Imath::M44d translateMatrix;
        translateMatrix.translate (diffTrans);
        assert (procrustesError (&from[0], &to[0], &weights[0], n, m * translateMatrix) >
                procrustesError (&from[0], &to[0], &weights[0], n, m));
    }

    // Try a small scale:
    Imath::M44d newMat = ms;
    const double scaleDiff = delta;
    for (size_t i = 0; i < 3; ++i)
        for (size_t j = 0; j < 3; ++j)
            newMat[i][j] = ms[i][j] * (1.0 + scaleDiff);
    assert (procrustesError (&from[0], &to[0], &weights[0], n, newMat) >
            procrustesError (&from[0], &to[0], &weights[0], n, ms));

    for (size_t i = 0; i < 3; ++i)
        for (size_t j = 0; j < 3; ++j)
            newMat[i][j] = ms[i][j] * (1.0 - scaleDiff);
    assert (procrustesError (&from[0], &to[0], &weights[0], n, newMat) >
            procrustesError (&from[0], &to[0], &weights[0], n, ms));

    //
    // Verify the magical property that makes shape springs work:
    // when the displacements Q*A-B, times the weights,
    // are applied as forces at B,
    // there is zero net force and zero net torque.
    //
    {
        Imath::V3d center (0, 0, 0);

        Imath::V3d netForce(0);
        Imath::V3d netTorque(0);
        for (int iPoint = 0; iPoint < n; ++iPoint)
        {
            const Imath::V3d force = weights[iPoint] * (from[iPoint]*m - to[iPoint]);
            netForce += force;
            netTorque += to[iPoint].cross (force);
        }

        assert (netForce.length2() < eps);
        assert (netTorque.length2() < eps);
    }
}