---

simSimulator.cpp

//////////////////////////////////////////////////////////////////////
// Copyright (c) 2002-2003 David Pritchard <drpritch@alumni.uwaterloo.ca>
// 
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public License
// as published by the Free Software Foundation; either
// version 2 of the License, or (at your option) any later
// version.
// 
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

#include <freecloth/simulator/simSimulator.h>
#include <freecloth/geom/geVector.h>
#include <freecloth/geom/geMatrix3.h>
#include <freecloth/base/fstream>
#include <freecloth/base/iomanip>
#include <freecloth/base/baMath.h>
#include <freecloth/base/baStringUtil.h>

#ifdef NDEBUG
    #define DO_DEBUG(x)
#else
    #define DO_DEBUG(x) x
#endif

// If set, avoid [BarWit98]'s assumption that stretch always keeps the
// edge lengths to a fixed size. Not very sure that it works...
#define DO_NORM             0
// If set, adjust [BarWit98]'s condition functions to be scale invariant
#define DO_SCALE_INVARIANT  1

namespace {
    using namespace freecloth;

    typedef Float Float_3[3];
    typedef Float Float_4[4];
    typedef GePoint GePoint_3[3];
    typedef GePoint GePoint_4[4];
    typedef GeVector GeVector_3[3];
    typedef GeVector GeVector_33[3][3];
    typedef GeVector GeVector_3333[3][3][3][3];
    typedef GeVector GeVector_4[4];
    typedef GeVector GeVector_43[4][3];
    typedef GeVector GeVector_443[4][4][3];
    typedef GeVector GeVector_4433[4][4][3][3];
    typedef GeMatrix3 GeMatrix3_33[3][3];
    typedef GeMatrix3 GeMatrix3_44[4][4];

    const bool PRINT_STATS = false;

    const bool DEBUG_REWIND = false;
    const bool DEBUG_MASS = false;
    const bool DEBUG_STEP = false;
    const bool DEBUG_COMMON = false;
    const bool VERIFY_COMMON = false;
    const bool DEBUG_STRETCH = false;
    const bool VERIFY_STRETCH = false;
    const bool DEBUG_SHEAR = false;
    const bool VERIFY_SHEAR = false;
    const bool DEBUG_BEND = false;
    // Verification - for each derivative da/db calculated, verify the
    // calculation by performing (a(b0+db)-a(b0))/db calculation, and asserting
    // that this matches the actual derivative calculation.
    const bool VERIFY_BEND = false;

    const bool DUMP_DISTS = false;
    const bool DUMP_FORCES = false;




//------------------------------------------------------------------------------

    inline GeVector makeSkewRow( const GeVector& v, UInt32 row )
    {
        DGFX_ASSERT( row < 3 );

        switch( row ) {
            case 0: 
                return GeVector( 0, -v._z, v._y );
            case 1:
                return GeVector( v._z, 0, -v._x );
            case 2:
                return GeVector( -v._y, v._x, 0 );
        }
        return GeVector::ZERO;
    }


//------------------------------------------------------------------------------

    template <class E1>
    bool isSymmetric( const E1& A )
    {
        DGFX_ASSERT( A.nbRows() == A.nbColumns() );
        for ( UInt32 i = 0; i < A.nbRows(); ++i ) {
            for ( UInt32 j = i + 1; j < A.nbRows(); ++j ) {
                if ( A( i, j ) != A( j, i ).transpose() ) {
                    return false;
                }
            }
        }
        return true;
    }
}

FREECLOTH_NAMESPACE_START

////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::CommonVars

/*!
 * \class SimSimulator::CommonVars
 * \brief Variables common to stretch and shear force computation.
 */
class SimSimulator::CommonVars
{
public:
    // ----- member functions -----

    void calc( const GePoint_3 p, const GePoint_3 tp );
    void debugOutput( std::ostream& os );

    // ----- member variables -----

    Float _du1,_dv1,_du2,_dv2;
    Float _a;
    GeVector _wu,_wv;
    //! Normalised versions of wu and wv ($\frac{wu}{\norm{\wu}}$)
    GeVector _whatu,_whatv;
    //! Inverse magnitude of wu and wv ($\frac{1}{\norm{\wu}}$)
    Float _wuim,_wvim;

    Float_3 _dwux_dxmx, _dwvx_dxmx;
#if DO_NORM
    GeVector_33 _dwhatu_dxm, _dwhatv_dxm;
    GeVector_3333 _d2whatu_dxmdxn, _d2whatv_dxmdxn;
#endif
};

////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::StretchVars

class SimSimulator::StretchVars {
public:
    // ----- member functions -----

    void calc(
        const CommonVars& cv,
        const GeVector v0[ 3 ],
        Float b_u, Float b_v
    );
    void debugOutput( std::ostream& os );

    // ----- member variables -----
    Float _Cu, _Cv;
    GeVector_3 _dCu_dxm, _dCv_dxm;
    Float _dCu_dt, _dCv_dt;
    GeMatrix3_33 _d2Cu_dxmdxn, _d2Cv_dxmdxn;
};

////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::ShearVars

/*!
 * \brief Variables used in shear force computation.
 */
class SimSimulator::ShearVars {
public:
    // ----- member functions -----

    void calc(
        const CommonVars& cv,
        const GeVector v0[ 3 ]
    );
    void debugOutput( std::ostream& os );

    // ----- member variables -----
    Float _C;
    GeVector_3 _dC_dxm;
    Float _dC_dt;
    GeMatrix3_33 _d2C_dxmdxn;
};


////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::BendVars

/*!
 * \brief Variables used in bend force computation.
 */
class SimSimulator::BendVars
{
public:
    // ----- member functions -----

    void calc(
        const GePoint_4 x,
        const GeVector_4 v
    );
    //! This variant is for use when verifying, especially without DO_NORM.
    void calc(
        const GePoint_4 x,
        const GeVector_4 v,
        const BendVars& bv
    );
    void debugOutput( std::ostream& os );

    // ----- member variables -----
    //! Unit vectors
    GeVector _nhatA, _nhatB, _ehat;
    //! Inverse magnitude of nA, nB, e
    Float _nAim, _nBim, _eim;
    Float _costh, _sinth;
    Float _C;

    GeVector_4 _qA, _qB;
    Float_4 _qe;
    GeVector_443 _dqAm_dxn, _dqBm_dxn;

    GeVector_43 _dnhatA_dxm, _dnhatB_dxm, _dehat_dxm;
    GeVector_4433 _d2nA_dxmdxn, _d2nB_dxmdxn;
#if DO_NORM
    GeVector_4433 _d2e_dxmdxn;
#endif

    GeVector_4 _dcosth_dxm, _dsinth_dxm;
    GeMatrix3_44 _d2costh_dxmdxn, _d2sinth_dxmdxn;

    GeVector_4 _dC_dxm;
    Float _dC_dt;
    GeMatrix3_44 _d2C_dxmdxn;

private:

    // ----- member functions -----
    void mainCalc( const GePoint_4 x, const GeVector_4 v );
    void calcNE( const GePoint_4 x );
    void calcThetas();
    void calcQs( const GePoint_4 x );
    void calcDqs();
    void calcNEderiv();
    void calcD2NE();
    void calcCosSinDeriv();
    void calcD2cossinth();
    void calcCderivs();
};


////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator

// LaTeX setup
//
/* $
\newcommand{\pfrac}[2]{
    \frac{\partial #1}{\partial #2}
}
\newcommand{\pfractwo}[3]{
    \frac{\partial^2 #1}{\partial #2 \partial #3}
}
\newcommand{\cross}{\times}
\newcommand{\I}{{\bf I}}
\newcommand{\Is}{{\bf I}_s}
\newcommand{\x}{{\bf x}}
\newcommand{\xm}{\x_m}
\newcommand{\xms}{\x_{m_s}}
\newcommand{\xmt}{\x_{m_t}}
\newcommand{\xmx}{\x_{m_x}}
\newcommand{\xn}{\x_n}
\newcommand{\xns}{\x_{n_s}}
\newcommand{\xnt}{\x_{n_t}}
\newcommand{\xnx}{\x_{n_x}}
\newcommand{\du}{\Delta u}
\newcommand{\dv}{\Delta v}
\newcommand{\C}{{\bf C}}
\newcommand{\Cu}{C_u}
\newcommand{\Cv}{C_v}
\newcommand{\w}{{\bf w}}
\newcommand{\what}{{\bf {\hat w}}}
\newcommand{\wu}{\w_u}
\newcommand{\wus}{\w_{u_s}}
\newcommand{\wut}{\w_{u_t}}
\newcommand{\wux}{\w_{u_x}}
\newcommand{\whatu}{\what_u}
\newcommand{\whatux}{\what_{u_x}}
\newcommand{\wv}{\w_v}
\newcommand{\wvs}{\w_{v_s}}
\newcommand{\wvt}{\w_{v_t}}
\newcommand{\wvx}{\w_{v_x}}
\newcommand{\whatv}{\what_v}
\newcommand{\whatvx}{\what_{v_x}}
\newcommand{\n}{{\bf n}}
\newcommand{\nhat}{{\bf {\hat n}}}
\newcommand{\e}{{\bf e}}
\newcommand{\ehat}{{\bf {\hat e}}}
\newcommand{\q}{{\bf q}}
$ */

//------------------------------------------------------------------------------

SimSimulator::SimSimulator( const GeMesh& initialMesh )
  : _initialMesh( new GeMesh( initialMesh ) ),
    _rho( .01f ),
    _h( .02f ),
    _time( 0.f )
{
    rewind();
    setupMass();
    removeAllConstraints();
}

//------------------------------------------------------------------------------

void SimSimulator::rewind()
{
    _initialMeshWingedEdge = RCShdPtr<GeMeshWingedEdge>(
        new GeMeshWingedEdge( _initialMesh )
    );
    if ( DEBUG_REWIND ) {
        std::cout << "winged edges = " << _initialMeshWingedEdge << std::endl;
    }
    _mesh = RCShdPtr<GeMesh>( new GeMesh( *_initialMesh ) );

    const UInt32 N = _mesh->getNbVertices();
    _v0 = SimVector( N );
    _f0 = SimVector( N );
    _df_dx = SimMatrix( N, N );
    _df_dv = SimMatrix( N, N );
    _modPCG._b = SimVector( N );
    _modPCG._A = SimMatrix( N, N );
    UInt32 i;
    ForceType ftypes[] = { F_STRETCH, F_SHEAR, F_BEND };
    for( i = 0; i < 3; ++i ) {
        UInt32 f( ftypes[ i ] );
        _f0i[ f ] = SimVector( N );
        _d0i[ f ] = SimVector( N );
        _df_dxi[ f ] = SimMatrix( N, N );
        _dd_dxi[ f ] = SimMatrix( N, N );
        _dd_dvi[ f ] = SimMatrix( N, N );

        _fenergy[ i ] = 0;
    }
    _f0i[ F_GRAVITY ] = SimVector( N );
    _f0i[ F_DRAG ] = SimVector( N );

    for( i = 0; i < NB_FORCES; ++i ) {
        _trienergy[ i ].resize( _initialMesh->getNbFaces() );
    }

    _venergy = 0;
    _energy = 0;
    _time = 0.f;
}

//------------------------------------------------------------------------------

void SimSimulator::setupMass()
{
    const UInt32 N = _mesh->getNbVertices();
    _M = SimMatrix( N, N );
    _totalMass = 0;

    GeMesh::FaceConstIterator fi;
    GeMesh::VertexId vid;
    for( fi = _initialMesh->beginFace(); fi != _initialMesh->endFace(); ++fi ) {
        Float m = _rho * fi->calcArea() / 3;
        for ( UInt32 i = 0; i < 3; ++i ) {
            vid = fi->getVertexId( i );
            // NOTE: taking a reference will *not* work with uBLAS
            GeMatrix3 M( _M( vid, vid ) );
            M( 0, 0 ) += m;
            M( 1, 1 ) += m;
            M( 2, 2 ) += m;
            _M( vid, vid ) = M;
            _totalMass += m;
        }
    }

    if ( DEBUG_MASS ) {
        std::cout << "_M = " << _M << std::endl;
        std::cout << "totalMass = " << _totalMass << std::endl;
    }
}

//------------------------------------------------------------------------------

void SimSimulator::removeAllConstraints()
{
    const UInt32 N = _mesh->getNbVertices();
    GeMesh::VertexId vid;
    _modPCG._S.resize( N );
    for( vid = 0; vid < N; ++vid ) {
        _modPCG._S[ vid ] = GeMatrix3::IDENTITY;
    }
    _z0 = SimVector( N );
    _z0.clear();

    // This is isn't strictly necessary, but generally gets nicer results
    // when the user interactively changes constraints.
    _v0.clear();
}

//------------------------------------------------------------------------------

void SimSimulator::setPosConstraintPlane(
    GeMesh::VertexId vid,
    const GeVector& p
) {
    GeVector pu( p.getUnit() );
    DGFX_ASSERT( vid < _modPCG._S.size() );
    _modPCG._S[ vid ] =
        GeMatrix3::IDENTITY - GeMatrix3::outerProduct( pu, pu );
}

//------------------------------------------------------------------------------

void SimSimulator::setPosConstraintLine(
    GeMesh::VertexId vid,
    const GeVector& v
) {
    GeVector vu( v.getUnit() );

    // Construct a vector perpendicular to l.
    GeVector p;
    if ( !BaMath::isEqual( vu._z, 0 ) ) {
        p = GeVector( 1, 1, ( vu._x + vu._y ) / vu._z );
    }
    else if ( !BaMath::isEqual( vu._y, 0 ) ) {
        p = GeVector( 1, ( vu._x + vu._z ) / vu._y, 1 );
    }
    else {
        p = GeVector( ( vu._y + vu._z ) / vu._x, 1, 1 );
    }
    GeVector pu( p.getUnit() );
    GeVector qu( vu.cross( pu ) );

    DGFX_ASSERT( BaMath::isEqual( qu.length(), 1 ) );
    DGFX_ASSERT( BaMath::isEqual( pu.dot( vu ), 0 ) );
    DGFX_ASSERT( BaMath::isEqual( qu.dot( vu ), 0 ) );
    DGFX_ASSERT( BaMath::isEqual( pu.dot( qu ), 0 ) );

    DGFX_ASSERT( vid < _modPCG._S.size() );
    _modPCG._S[ vid ] =
        GeMatrix3::IDENTITY - GeMatrix3::outerProduct( pu, pu ) -
        GeMatrix3::outerProduct( qu, qu );
}

//------------------------------------------------------------------------------

void SimSimulator::setPosConstraintFull( GeMesh::VertexId vid )
{
    DGFX_ASSERT( vid < _modPCG._S.size() );
    _modPCG._S[ vid ] =
        GeMatrix3::ZERO;
}

//------------------------------------------------------------------------------

void SimSimulator::setVelConstraint( GeMesh::VertexId vid, const GeVector& v )
{
    DGFX_ASSERT( vid < _z0.size() );
    _z0[ vid ] = v;
}

//------------------------------------------------------------------------------

void SimSimulator::step()
{
    preSubSteps();
    while ( ! subStepsDone() ) {
        subStep();
    }
    postSubSteps();
}

//------------------------------------------------------------------------------

void SimSimulator::preSubSteps()
{
    UInt32 i;

    if ( PRINT_STATS ) {
        std::cout << "****************************************" << std::endl;
        std::cout << "Simulating at time " << _time << std::endl;
    }

    _fenergy[ F_STRETCH ] = 0;
    _fenergy[ F_SHEAR ] = 0;
    _fenergy[ F_BEND ] = 0;

    _venergy = .5 * _v0.dot( _M * _v0 );

    // Clear all forces
    _f0.clear();

    // *= 0 is better than clear() here - it retains the sparsity pattern
    // within the matrix, to avoid unnecessary reallocations after the first
    // call to step().
    _df_dx *= 0;
    _df_dv *= 0;
    for( i = 0; i < NB_FORCES; ++i ) {
        _f0i[ i ].clear();
        _d0i[ i ].clear();
        _df_dxi[ i ] *= 0;
        _dd_dxi[ i ] *= 0;
        _dd_dvi[ i ] *= 0;
    }
    _modPCG._A *= 0;
    _modPCG._b.clear();

    GeMesh::FaceConstIterator fi;
    for( fi = _mesh->beginFace(); fi != _mesh->endFace(); ++fi ) {
        calcStretchShear( *fi );
    }
    GeMeshWingedEdge::EdgeIterator ei;

    for(
        ei = _initialMeshWingedEdge->beginEdge();
        ei != _initialMeshWingedEdge->endEdge();
        ++ei
    ) {
        // Only do edges that aren't on the boundary.
        if ( ei->hasTwin() ) {
            calcBend( *ei );
        }
    }

    for( i = 0; i < _mesh->getNbVertices(); ++i ) {
        const GeMatrix3& M = _M( i, i );
        Float gravity = -M( 2, 2 ) * _params._g;
        _f0[ i ]._z += gravity;
        DO_DEBUG( _f0i[ F_GRAVITY ][ i ]._z = gravity );
    }

    for ( fi = _mesh->beginFace(); fi != _mesh->endFace(); ++fi ) {
        Float area( fi->calcArea() );
        const GeVector normal( fi->calcNormal() );
        GeMesh::FaceVertexId fvid;
        for ( fvid = 0; fvid < fi->getNbVertices(); ++fvid ) {
            UInt32 vid( fi->getVertexId( fvid ) );
            Float vel( _v0[ vid ].dot( normal ) );
            const GeVector force( ( -_params._k_drag * vel * area ) * normal );
            _f0[ vid ] += force;
            DO_DEBUG( _f0i[ F_DRAG ][ vid ] += force );
        }
    }

    _energy = _venergy + _fenergy[ F_STRETCH ] +
        _fenergy[ F_SHEAR ] + _fenergy[ F_BEND ];
    if ( DEBUG_STEP ) {
        for ( UInt32 i = 0; i < NB_FORCES; ++i ) {
            const char* s[] = { "stretch","shear","bend","gravity","drag" };
            std::cout << "f0[" << s[i] << "] = " << _f0i[ i ] << std::endl;
            std::cout << std::endl;
            std::cout << "d0[" << s[i] << "] = " << _d0i[ i ] << std::endl;
            std::cout << std::endl;
            std::cout << "df_dx[" << s[i] <<"] = " << _df_dxi[ i ] << std::endl;
            std::cout << std::endl;
            std::cout << "dd_dx[" << s[i] <<"] = " << _dd_dxi[ i ] << std::endl;
            std::cout << std::endl;
            std::cout << "dd_dv[" << s[i] <<"] = " << _dd_dvi[ i ] << std::endl;
            std::cout << std::endl;
            std::cout << std::endl;
        }
    }

    if ( DEBUG_STEP ) {
        std::cout << "f0 = " << _f0 << std::endl;
        std::cout << "df_dx = " << _df_dx << std::endl;
        std::cout << "df_dv = " << _df_dv << std::endl;
    }

    // Paper eq. (16)
    _modPCG._A = _df_dx * (-_h * _h) + _df_dv * -_h + _M;
    _modPCG._b = _h * ( _h * _df_dx * _v0 + _f0 );

    if ( DEBUG_STEP ) {
        std::cout << "A = " << _modPCG._A << std::endl;
        std::cout << "b = " << _modPCG._b << std::endl;
    }

    // Now, we're left with a system Ax=b, where x corresponds to deltav
    
    _modPCG._z = _h * _z0;
    _modPCG.preStep();
}

//------------------------------------------------------------------------------

bool SimSimulator::subStepsDone() const
{
    return _modPCG.done();
}

//------------------------------------------------------------------------------

void SimSimulator::subStep()
{
    _modPCG.step();
}

//------------------------------------------------------------------------------

void SimSimulator::postSubSteps()
{
    const SimVector& x = _modPCG.result();
    if ( DEBUG_STEP ) {
        std::cout << "deltav = " << x << std::endl;
        std::cout << "Ax = " << _modPCG._A * x << std::endl;
    }

    _v0 += x;

    GeMesh::VertexIterator vi;
    UInt32 i;
    for ( vi = _mesh->beginVertex(), i = 0; vi != _mesh->endVertex(); ++vi, ++i ) {
        (*vi) += _h * _v0[ i ];
    }
    _time += _h;
    if ( PRINT_STATS ) {
        std::cout.precision( 4 );
        std::cout.width( 6 );
        std::cout.setf( std::ios::fixed );
        std::cout << "Total energy: "
            << std::setprecision( 5 ) << std::setw( 7 )
            << std::setiosflags( std::ios::fixed )
            << _energy
            << "( stretch: " << _fenergy[ F_STRETCH ]
            << " / shear: " << _fenergy[ F_SHEAR ]
            << " / bend: " << _fenergy[ F_BEND ]
            //<< " / kinetic: " << _venergy
            << ")" << std::resetiosflags( std::ios::fixed ) << std::endl;
        std::cout << std::endl;
    }

    if ( DUMP_DISTS ) {
        static std::ofstream* fptr;
        if ( fptr == 0 ) {
            Float distance( (
                _initialMesh->getVertex( 1 ) - _initialMesh->getVertex( 0 )
            ).length() * 3 );
            String filename(
                "dists-" + BaStringUtil::fromFloat( distance, 1 ) + ".txt"
            );
            fptr = new std::ofstream( filename.c_str() );
        }
        std::ofstream& out = *fptr;
        GeMesh::VertexConstIterator vbi = _initialMesh->beginVertex();
        GeMesh::VertexConstIterator vi = _mesh->beginVertex();
        for ( ; vi != _mesh->endVertex(); ++vi, ++vbi ) {
            out << ( *vi - *vbi ).length() << " ";
        }
        out << std::endl;
    }

    if ( DUMP_FORCES ) {
        static std::ofstream* fptr;
        if ( fptr == 0 ) {
            Float distance( (
                _initialMesh->getVertex( 1 ) - _initialMesh->getVertex( 0 )
            ).length() * 3 );
            String filename(
                "forces-" + BaStringUtil::fromFloat( distance, 2 ) + ".txt"
            );
            fptr = new std::ofstream( filename.c_str() );
        }
        std::ofstream& out = *fptr;
        for ( UInt32 i = 0; i < _f0i[ F_STRETCH ].size(); ++i ) {
            out << _f0i[ F_STRETCH ][ i ].length() << " ";
            out << _f0i[ F_SHEAR ][ i ].length() << " ";
            out << _f0i[ F_BEND ][ i ].length() << " ";
            out << _d0i[ F_STRETCH ][ i ].length() << " ";
            out << _d0i[ F_SHEAR ][ i ].length() << " ";
            out << _d0i[ F_BEND ][ i ].length() << " ";
        }
        out << std::endl;
    }
}

//------------------------------------------------------------------------------

const GeMesh& SimSimulator::getMesh() const
{
    return *_mesh;
}

//------------------------------------------------------------------------------

const RCShdPtr<GeMesh>& SimSimulator::getMeshPtr() const
{
    return _mesh;
}

//------------------------------------------------------------------------------

BaTime::Instant SimSimulator::getTime() const
{
    return BaTime::floatAsInstant( _time );
}

//------------------------------------------------------------------------------

void SimSimulator::setTimestep( Float h )
{
    _h = h;
}

//------------------------------------------------------------------------------

void SimSimulator::setDensity( Float rho )
{
    _rho = rho;
    setupMass();
}

//------------------------------------------------------------------------------

void SimSimulator::setPCGTolerance( Float tolerance )
{
    _modPCG.setTolerance( tolerance );
}

//------------------------------------------------------------------------------

void SimSimulator::setParams( const Params& params )
{
    _params = params;
}

//------------------------------------------------------------------------------

Float SimSimulator::getTimestep() const
{
    return _h;
}

//------------------------------------------------------------------------------

Float SimSimulator::getDensity() const
{
    return _rho;
}

//------------------------------------------------------------------------------

Float SimSimulator::getPCGTolerance() const
{
    return _modPCG.getTolerance();
}

//------------------------------------------------------------------------------

const SimSimulator::Params& SimSimulator::getParams() const
{
    return _params;
}

//------------------------------------------------------------------------------

GeVector SimSimulator::getVelocity( GeMesh::VertexId vid ) const
{
    DGFX_ASSERT( vid < _v0.size() );
    return _v0[ vid ];
}

//------------------------------------------------------------------------------

GeVector SimSimulator::getForce( GeMesh::VertexId vid ) const
{
    DGFX_ASSERT( vid < _f0.size() );
    return _f0[ vid ];
}

//------------------------------------------------------------------------------

GeVector SimSimulator::getForce( ForceType type, GeMesh::VertexId vid ) const
{
    DGFX_ASSERT( type < NB_FORCES );
    DGFX_ASSERT( vid < _f0i[ type ].size() );
    return _f0i[ type ][ vid ];
}

//------------------------------------------------------------------------------

GeVector SimSimulator::getDampingForce(
    ForceType type,
    GeMesh::VertexId vid
) const {
    DGFX_ASSERT( type < NB_FORCES );
    DGFX_ASSERT( vid < _d0i[ type ].size() );
    return _d0i[ type ][ vid ];
}

//------------------------------------------------------------------------------

Float SimSimulator::getEnergy( ForceType type ) const
{
    DGFX_ASSERT( type < NB_FORCES );
    return _fenergy[ type ];
}

//------------------------------------------------------------------------------

Float SimSimulator::getTriEnergy( ForceType type, GeMesh::FaceId fid ) const
{
    DGFX_ASSERT( type < F_GRAVITY );
    DGFX_ASSERT( fid < _trienergy[ type ].size() );
    return _trienergy[ type ][ fid ];
}

//------------------------------------------------------------------------------

void SimSimulator::setMesh( const GeMesh& mesh )
{
    DGFX_ASSERT( mesh.getNbVertices() == _initialMesh->getNbVertices() );
    DGFX_ASSERT( mesh.getNbFaces() == _initialMesh->getNbFaces() );
    _mesh = RCShdPtr<GeMesh>( new GeMesh( mesh ) );
}



//------------------------------------------------------------------------------

void SimSimulator::calcStretchShear(
    const GeMesh::FaceWrapper& face
) {
    UInt32 m;

    // My x[0],x[1],x[2] are equivalent to [BarWit98]'s xi,xj,xk.
    GeMesh::VertexType x[3];
    GeMesh::TextureVertexType tp[3];
    for( m = 0; m < 3; ++m ) {
        x[ m ] = face.getVertex( m );
        tp[ m ] = _initialMesh->getTextureVertex( face.getVertexId( m ) );
    }
    if ( DEBUG_COMMON ) {
        std::cout << "x0 = " << x[0] << " x1 = " << x[1]
            << " x2 = " << x[2] << std::endl;
    }

    GeVector v0[ 3 ];
    for( m = 0; m < 3; ++m ) {
        UInt32 vid = face.getVertexId( m );
        v0[ m ] = _v0[ vid ];
    }

    CommonVars cv;
    cv.calc( x, tp );
    if ( DEBUG_COMMON ) {
        cv.debugOutput( std::cout );
    }

    calcStretch( face, cv, x, tp, v0 );
    calcShear( face, cv, x, tp, v0 );

    if ( VERIFY_COMMON ) {
#if DO_NORM
        const Float MAX_VERIFY_ERROR = .01; // maximum allowed
        const Float MAX_VERIFY_ERROR_M = MAX_VERIFY_ERROR * 3; // for matrices
        const Float VERIFY_STEP = .001;     // db
        CommonVars ver_cv[3][3];
        for( m = 0; m < 3; ++m ) for( UInt32 s = 0; s < 3; ++s ) {
            GePoint testx[3] = { x[0], x[1], x[2] };
            testx[ m ][ s ] += VERIFY_STEP;
            ver_cv[ m ][ s ].calc( testx, tp );
        }

        bool DISPLAY;

        UInt32 n,s,t;
        DISPLAY = false;
        for( m = 0; m < 3; ++m ) for( UInt32 s = 0; s < 3; ++s ) {
            const CommonVars& testcv = ver_cv[m][s];

            GeVector test_dwhatu_dxm = ( testcv._whatu - cv._whatu ) / VERIFY_STEP;
            GeVector test_dwhatv_dxm = ( testcv._whatv - cv._whatv ) / VERIFY_STEP;

            Float err;

            if ( DISPLAY ) {
                std::cout << "test_dwhatu_dxm[ " << m << " ][ " << s << " ] = "
                    << test_dwhatu_dxm << std::endl;
            }
            err = ( test_dwhatu_dxm - cv._dwhatu_dxm[m][s] ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR );

            if ( DISPLAY ) {
                std::cout << "test_dwhatv_dxm[ " << m << " ][ " << s << " ] = "
                    << test_dwhatv_dxm << std::endl;
            }
            err = ( test_dwhatv_dxm - cv._dwhatv_dxm[m][s] ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR );
        }

        DISPLAY = false;
        for ( n = 0; n < 3; ++n ) for( m = 0; m < 3; ++m ) {
            for( s = 0; s < 3; ++s ) for( t = 0; t < 3; ++t ) {
                const CommonVars& testcv = ver_cv[n][t];
                GeVector test_d2whatu_dxmdxn =
                    (testcv._dwhatu_dxm[m][s] - cv._dwhatu_dxm[m][s]) / VERIFY_STEP;
                GeVector test_d2whatv_dxmdxn =
                    (testcv._dwhatv_dxm[m][s] - cv._dwhatv_dxm[m][s]) / VERIFY_STEP;

                Float err;

                if ( DISPLAY ) {
                    std::cout << "test_d2whatu_dxmdxn[" << m << "][" << n << "]["
                        << s << "][" << t << "] = "
                        << test_d2whatu_dxmdxn << std::endl;
                }
                err = (
                    test_d2whatu_dxmdxn - cv._d2whatu_dxmdxn[m][n][s][t]
                ).infinityNorm();
                DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );

                if ( DISPLAY ) {
                    std::cout << "test_d2whatv_dxmdxn[" << m << "][" << n << "]["
                        << s << "][" << t << "] = "
                        << test_d2whatv_dxmdxn << std::endl;
                }
                err = (
                    test_d2whatv_dxmdxn - cv._d2whatv_dxmdxn[m][n][s][t]
                ).infinityNorm();
                DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
            }
        }
#endif

    }
}


//------------------------------------------------------------------------------

void SimSimulator::calcStretch(
    const GeMesh::FaceWrapper& face,
    const CommonVars &cv,
    const GePoint x[3],
    const GePoint tp[3],
    const GeVector v0[ 3 ]
) {
    const Float MAX_VERIFY_ERROR = .01f; // maximum allowed
    const Float MAX_VERIFY_ERROR_M = MAX_VERIFY_ERROR * 3; // for matrices
    const Float VERIFY_STEP = .001f;     // db
    UInt32 m,n,s,t;

    StretchVars stv;
    stv.calc( cv, v0, _params._b_u, _params._b_v );

    if ( DEBUG_STRETCH ) {
        stv.debugOutput( std::cout );
    }

    for ( m = 0; m < 3; ++m ) {
        UInt32 vid = face.getVertexId( m );
        GeVector val;

        // As per [BarWit98] eq. (7)
        val = -_params._k_stretch * (
            stv._dCu_dxm[ m ] * stv._Cu + stv._dCv_dxm[ m ] * stv._Cv
        );
        _f0[ vid ] += val;
        DO_DEBUG( _f0i[ F_STRETCH ][ vid ] += val );
        if ( DEBUG_STRETCH ) {
            std::cout << "f0[stretch][ " << m << " ] = " << val << std::endl;
        }

        val = -_params._k_damp * _params._k_stretch * (
            stv._dCu_dxm[ m ] * stv._dCu_dt
            + stv._dCv_dxm[ m ] * stv._dCv_dt
        );
        _f0[ vid ] += val;

        DO_DEBUG( _d0i[ F_STRETCH ][ vid ] += val );
        if ( DEBUG_STRETCH ) {
            std::cout << "d0[stretch][ " << m << " ] = " << val << std::endl;
        }
    }
    for ( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
        UInt32 vidm = face.getVertexId( m );
        UInt32 vidn = face.getVertexId( n );
        GeMatrix3 val;

        // As per [BarWit98] eq. (8)
        val = -_params._k_stretch * (
            GeMatrix3::outerProduct( stv._dCu_dxm[ m ], stv._dCu_dxm[ n ] )
            + GeMatrix3::outerProduct( stv._dCv_dxm[ m ], stv._dCv_dxm[ n ] )
            + stv._d2Cu_dxmdxn[ m ][ n ] * stv._Cu
            + stv._d2Cv_dxmdxn[ m ][ n ] * stv._Cv
        );
        _df_dx[ vidn ][ vidm ] += val;
        if ( DEBUG_STRETCH ) {
            _df_dxi[ F_STRETCH ][ vidn ][ vidm ] += val;
            std::cout << "df_dx[stretch][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }

        val = -_params._k_damp * _params._k_stretch * (
            stv._d2Cu_dxmdxn[ m ][ n ] * stv._dCu_dt
            + stv._d2Cv_dxmdxn[ m ][ n ] * stv._dCv_dt
        );
        _df_dx[ vidn ][ vidm ] += val;
        if ( DEBUG_STRETCH ) {
            _dd_dxi[ F_STRETCH ][ vidn ][ vidm ] += val;
            std::cout << "dd_dx[stretch][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }

        val = -_params._k_damp * _params._k_stretch * (
            GeMatrix3::outerProduct( stv._dCu_dxm[m], stv._dCu_dxm[n] )
            + GeMatrix3::outerProduct( stv._dCv_dxm[m], stv._dCv_dxm[n] )
        );
        _df_dv[ vidn ][ vidm ] += val;
        if ( DEBUG_STRETCH ) {
            _dd_dvi[ F_STRETCH ][ vidn ][ vidm ] += val;
            std::cout << "dd_dv[stretch][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }
    }

    Float E =
        _params._k_stretch * .5 * ( stv._Cu * stv._Cu + stv._Cv * stv._Cv );
    _trienergy[ F_STRETCH ][ face.getFaceId() ] = E;
    _fenergy[ F_STRETCH ] += E;

    if ( DEBUG_STRETCH ) {
        std::cout << std::endl;
    }

    if ( VERIFY_STRETCH ) {
        StretchVars ver_stv[3][3];
        for( m = 0; m < 3; ++m ) for( UInt32 s = 0; s < 3; ++s ) {
            GePoint testx[3] = { x[0], x[1], x[2] };
            testx[ m ][ s ] += VERIFY_STEP;
            CommonVars ver_cv;
            ver_cv.calc( testx, tp );
            ver_stv[ m ][ s ].calc( ver_cv, v0, _params._b_u, _params._b_v );
        }
        bool DISPLAY;

        DISPLAY = false;
        for( m = 0; m < 3; ++m ) {
            GeVector testdCu_dxm, testdCv_dxm;
            for( UInt32 s = 0; s < 3; ++s ) {
                const StretchVars& teststv = ver_stv[m][s];
                testdCu_dxm[ s ] = ( teststv._Cu - stv._Cu ) / VERIFY_STEP;
                testdCv_dxm[ s ] = ( teststv._Cv - stv._Cv ) / VERIFY_STEP;
            }
            Float err;
            if ( DISPLAY ) {
                std::cout << "testdCu_dxm[ " << m << " ] = " << testdCu_dxm << std::endl;
                std::cout << "testdCv_dxm[ " << m << " ] = " << testdCv_dxm << std::endl;
            }
            err = (
                testdCu_dxm - stv._dCu_dxm[ m ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
            err = (
                testdCv_dxm - stv._dCv_dxm[ m ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
        }

        DISPLAY = false;
        for( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
            GeMatrix3 testd2Cu_dxmdxn, testd2Cv_dxmdxn;
            for( t = 0; t < 3; ++t ) {
                const StretchVars& teststv = ver_stv[n][t];
                const GeVector diffu(
                    teststv._dCu_dxm[ m ] - stv._dCu_dxm[ m ]
                );
                const GeVector diffv(
                    teststv._dCv_dxm[ m ] - stv._dCv_dxm[ m ]
                );
                for( s = 0; s < 3; ++s ) {
                    testd2Cu_dxmdxn( t, s ) = diffu[ s ] / VERIFY_STEP;
                    testd2Cv_dxmdxn( t, s ) = diffv[ s ] / VERIFY_STEP;
                }
            }

            Float err;
            if ( DISPLAY ) {
                std::cout << "testd2Cu_dxmdxn[ " << m << " ][ " << n
                    << " ] = " << testd2Cu_dxmdxn << std::endl;
            }
            err = (
                testd2Cu_dxmdxn - stv._d2Cu_dxmdxn[ m ][ n ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );

            if ( DISPLAY ) {
                std::cout << "testd2Cv_dxmdxn[ " << m << " ][ " << n
                    << " ] = " << testd2Cv_dxmdxn << std::endl;
            }
            err = (
                testd2Cv_dxmdxn - stv._d2Cv_dxmdxn[ m ][ n ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
        }
    }
}

//------------------------------------------------------------------------------

void SimSimulator::calcShear(
    const GeMesh::FaceWrapper& face,
    const CommonVars &cv,
    const GePoint x[4],
    const GePoint tp[4],
    const GeVector v0[ 3 ]
) {
    const Float MAX_VERIFY_ERROR = .01f; // maximum allowed
    const Float MAX_VERIFY_ERROR_M = MAX_VERIFY_ERROR * 3; // for matrices
    const Float VERIFY_STEP = .001f;     // db
    UInt32 m,n,s,t;

    ShearVars shv;
    shv.calc( cv, v0 );

    if ( DEBUG_SHEAR ) {
        shv.debugOutput( std::cout );
    }

    for ( m = 0; m < 3; ++m ) {
        UInt32 vid = face.getVertexId( m );
        GeVector val;

        val = -_params._k_shear * shv._dC_dxm[ m ] * shv._C;
        _f0[ vid ] += val;
        DO_DEBUG( _f0i[ F_SHEAR ][ vid ] += val );
        if ( DEBUG_SHEAR ) {
            std::cout << "f0[shear][ " << m << " ] = " << val << std::endl;
        }

        val = -_params._k_damp * _params._k_shear
            * shv._dC_dxm[ m ] * shv._dC_dt;
        _f0[ vid ] += val;

        DO_DEBUG( _d0i[ F_SHEAR ][ vid ] += val );
        if ( DEBUG_SHEAR ) {
            std::cout << "d0[shear][ " << m << " ] = " << val << std::endl;
        }
    }
    for ( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
        UInt32 vidm = face.getVertexId( m );
        UInt32 vidn = face.getVertexId( n );
        GeMatrix3 val;
        val = -_params._k_shear * (
            GeMatrix3::outerProduct( shv._dC_dxm[ m ], shv._dC_dxm[ n ] )
            + shv._d2C_dxmdxn[ m ][ n ] * shv._C
        );
        _df_dx[ vidn ][ vidm ] += val;
        if ( DEBUG_SHEAR ) {
            _df_dxi[ F_SHEAR ][ vidn ][ vidm ] += val;
            std::cout << "df_dx[shear][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }

        val = -_params._k_damp * _params._k_shear
            * shv._d2C_dxmdxn[ m ][ n ] * shv._dC_dt;
        _df_dx[ vidn ][ vidm ] += val;
        if ( DEBUG_SHEAR ) {
            _dd_dxi[ F_SHEAR ][ vidn ][ vidm ] += val;
            std::cout << "dd_dx[shear][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }

        val = -_params._k_damp * _params._k_shear
            * GeMatrix3::outerProduct( shv._dC_dxm[ m ], shv._dC_dxm[ n ] );
        _df_dv[ vidn ][ vidm ] += val;

        if ( DEBUG_SHEAR ) {
            _dd_dvi[ F_SHEAR ][ vidn ][ vidm ] += val;
            std::cout << "dd_dv[shear][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }
    }

    Float E = _params._k_shear * .5 * shv._C * shv._C;
    _trienergy[ F_SHEAR ][ face.getFaceId() ] = E;
    _fenergy[ F_SHEAR ] += E;

    if ( DEBUG_SHEAR ) {
        std::cout << std::endl;
    }

    if ( VERIFY_SHEAR ) {
        ShearVars ver_shv[3][3];
        for( m = 0; m < 3; ++m ) for( UInt32 s = 0; s < 3; ++s ) {
            GePoint testx[3] = { x[0], x[1], x[2] };
            testx[ m ][ s ] += VERIFY_STEP;
            CommonVars ver_cv;
            ver_cv.calc( testx, tp );
            ver_shv[ m ][ s ].calc( ver_cv, v0 );
        }
        bool DISPLAY;

        DISPLAY = false;
        for( m = 0; m < 3; ++m ) {
            GeVector testdC_dxm;
            for( UInt32 s = 0; s < 3; ++s ) {
                const ShearVars& testshv = ver_shv[m][s];
                testdC_dxm[ s ] = ( testshv._C - shv._C ) / VERIFY_STEP;
            }
            Float err;
            if ( DISPLAY ) {
                std::cout << "testdC_dxm[ " << m << " ] = " << testdC_dxm << std::endl;
            }
            err = (
                testdC_dxm - shv._dC_dxm[ m ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
        }

        DISPLAY = false;
        for( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
            GeMatrix3 testd2C_dxmdxn;
            for( t = 0; t < 3; ++t ) {
                const ShearVars& testshv = ver_shv[n][t];
                const GeVector diff( testshv._dC_dxm[ m ] - shv._dC_dxm[ m ] );
                for( s = 0; s < 3; ++s ) {
                    testd2C_dxmdxn( t, s ) = diff[ s ] / VERIFY_STEP;
                }
            }

            Float err;
            if ( DISPLAY ) {
                std::cout << "testd2C_dxmdxn[ " << m << " ][ " << n
                    << " ] = " << testd2C_dxmdxn << std::endl;
            }
            err = (
                testd2C_dxmdxn - shv._d2C_dxmdxn[ m ][ n ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
        }
    }
}

//------------------------------------------------------------------------------

void SimSimulator::calcBend(
    const GeMeshWingedEdge::HalfEdgeWrapper& edge
) {
    const Float MAX_VERIFY_ERROR = .01f; // maximum allowed
    const Float MAX_VERIFY_ERROR_M = MAX_VERIFY_ERROR * 3; // for matrices
    const Float VERIFY_STEP = .001f;     // db
    UInt32 m, n, s, t;

    GeMeshWingedEdge::HalfEdgeWrapper he_next( edge.getNextHalfEdge() );
    GeMeshWingedEdge::HalfEdgeWrapper he_prev( edge.getPrevHalfEdge() );
    GeMeshWingedEdge::HalfEdgeWrapper he_twin( edge.getTwinHalfEdge() );

    GeMesh::VertexId vid[ 4 ];
    vid[ 0 ] = he_prev.getOriginVertexId();
    vid[ 1 ] = edge.getOriginVertexId();
    vid[ 2 ] = he_next.getOriginVertexId();
    vid[ 3 ] = he_twin.getPrevHalfEdge().getOriginVertexId();
    GeMesh::VertexType x[ 4 ];
    GeMesh::TextureVertexType tv[ 4 ];
    GeVector v[ 4 ];
    for( m = 0; m < 4; ++m ) {
        x[ m ] = _mesh->getVertex( vid[ m ] );
        tv[ m ] = _initialMesh->getTextureVertex( vid[ m ] );
        v[ m ] = _v0[ vid[ m ] ];
    }

    const Float du = tv[ 1 ]._x - tv[ 2 ]._x;
    const Float dv = tv[ 1 ]._y - tv[ 2 ]._y;
    const Float k = (
        _params._k_bend_u * du*du + _params._k_bend_v * dv*dv ) /
        (du*du + dv*dv);

    BendVars bv;
    bv.calc( x, v );
    if ( DEBUG_BEND ) {
        std::cout << "x0 = " << x[0] << "  x1 = " << x[1]
            << "  x2 = " << x[2] << "  x3 = " << x[3] << std::endl;
        std::cout << "k = " << k << std::endl;
        bv.debugOutput( std::cout );
    }

    // Same finale as stretch/shear
    for ( m = 0; m < 4; ++m ) {
        GeVector val;

        val = -k * bv._dC_dxm[ m ] * bv._C;
        _f0[ vid[ m ] ] += val;
        DO_DEBUG( _f0i[ F_BEND ][ vid[ m ] ] += val );
        if ( DEBUG_BEND ) {
            std::cout << "f0[bend][ " << m << " ] = " << val << std::endl;
        }

        val = -_params._k_damp * k * bv._dC_dxm[ m ] * bv._dC_dt;
        _f0[ vid[ m ] ] += val;

        DO_DEBUG( _d0i[ F_BEND ][ vid[ m ] ] += val );
        if ( DEBUG_BEND ) {
            std::cout << "d0[bend][ " << m << " ] = " << val << std::endl;
        }
    }

    Float E = .5 * k * bv._C * bv._C;
    // Spread energy to both triangles for debugging
    _trienergy[ F_BEND ][ edge.getFaceId() ] += E * .5;
    _trienergy[ F_BEND ][ he_twin.getFaceId() ] += E * .5;
    _fenergy[ F_BEND ] += E;

    for ( m = 0; m < 4; ++m ) for ( n = 0; n < 4; ++n ) {
        GeMatrix3 val;
        val = -k * (
            GeMatrix3::outerProduct( bv._dC_dxm[ m ], bv._dC_dxm[ n ] )
            + bv._d2C_dxmdxn[ m ][ n ] * bv._C
        );
        _df_dx[ vid[ n ] ][ vid[ m ] ] += val;
        if ( DEBUG_BEND ) {
            _df_dxi[ F_BEND ][ vid[ n ] ][ vid[ m ] ] += val;
            std::cout << "df_dx[bend][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }

        val = -_params._k_damp * k
            * bv._d2C_dxmdxn[ m ][ n ] * bv._dC_dt;
        _df_dx[ vid[ n ] ][ vid[ m ] ] += val;
        if ( DEBUG_BEND ) {
            _dd_dxi[ F_BEND ][ vid[ n ] ][ vid[ m ] ] += val;
            std::cout << "dd_dx[bend][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }

        val = -_params._k_damp * k
            * GeMatrix3::outerProduct( bv._dC_dxm[ m ], bv._dC_dxm[ n ] );
        _df_dv[ vid[ n ] ][ vid[ m ] ] += val;

        if ( DEBUG_BEND ) {
            _dd_dvi[ F_BEND ][ vid[ n ] ][ vid[ m ] ] += val;
            std::cout << "dd_dv[bend][ " << m << " ][ " << n << " ] = "
                << val << std::endl;
        }
    }
    if ( DEBUG_BEND ) {
        std::cout << std::endl << std::endl;
    }

    if ( VERIFY_BEND ) {
        BendVars ver_bv[4][3];
        for( m = 0; m < 4; ++m ) for( UInt32 s = 0; s < 3; ++s ) {
            GePoint testx[4] = { x[0], x[1], x[2], x[3] };
            testx[ m ][ s ] += VERIFY_STEP;
            ver_bv[ m ][ s ].calc( testx, v, bv );
        }
        bool DISPLAY;

        DISPLAY = false;
        for( m = 0; m < 4; ++m ) for( UInt32 s = 0; s < 3; ++s ) {
            const BendVars& testbv = ver_bv[m][s];

            GeVector test_dnhatA_dxm =
                ( testbv._nhatA - bv._nhatA ) / VERIFY_STEP;
            GeVector test_dnhatB_dxm =
                ( testbv._nhatB - bv._nhatB ) / VERIFY_STEP;
            GeVector test_dehat_dxm = ( testbv._ehat - bv._ehat ) / VERIFY_STEP;

            Float err;

            if ( DISPLAY ) {
                std::cout << "test_dnhatA_dxm[ " << m << " ][ " << s << "] = "
                    << test_dnhatA_dxm << std::endl;
            }
            err = ( test_dnhatA_dxm - bv._dnhatA_dxm[m][s] ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR );

            if ( DISPLAY ) {
                std::cout << "test_dnhatB_dxm[ " << m << " ][ " << s << "] = "
                    << test_dnhatB_dxm << std::endl;
            }
            err = ( test_dnhatB_dxm - bv._dnhatB_dxm[m][s] ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR );

            if ( DISPLAY ) {
                std::cout << "test_dehat_dxm[ " << m << " ][ " << s << "] = "
                    << test_dehat_dxm << std::endl;
            }
            err = ( test_dehat_dxm - bv._dehat_dxm[m][s] ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR );
        }

        DISPLAY = false;
        for( n = 0; n < 4; ++n ) {
            GeVector test_d2nA_dxmdxn[ 4 ][ 3 ][ 3 ];
            GeVector test_d2nB_dxmdxn[ 4 ][ 3 ][ 3 ];
            GeVector test_d2e_dxmdxn[ 4 ][ 3 ][ 3 ];
            for( t = 0; t < 3; ++t ) {
                const BendVars& testbv = ver_bv[n][t];
                for( m = 0; m < 4; ++m ) for( s = 0; s < 3; ++s ) {
                    test_d2nA_dxmdxn[ m ][ s ][ t ] = (
                        testbv._dnhatA_dxm[ m ][ s ] - bv._dnhatA_dxm[ m ][ s ]
                    ) / VERIFY_STEP;
                    test_d2nB_dxmdxn[ m ][ s ][ t ] = (
                        testbv._dnhatB_dxm[ m ][ s ] - bv._dnhatB_dxm[ m ][ s ]
                    ) / VERIFY_STEP;
                    test_d2e_dxmdxn[ m ][ s ][ t ] = (
                        testbv._dehat_dxm[ m ][ s ] - bv._dehat_dxm[ m ][ s ]
                    ) / VERIFY_STEP;
                }
            }
            Float err;
            for ( m = 0; m < 4; ++m ) {
                for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
                    if ( DISPLAY ) {
                        std::cout << "test_d2nA_dxmdxn[ " << m << " ][ "
                            << n << " ][ " << s << " ][ " << t << " ] = "
                            << test_d2nA_dxmdxn[ m ][ s ][ t ] << std::endl;
                    }
                    test_d2nA_dxmdxn[m][s][t] -= bv._d2nA_dxmdxn[m][n][s][t];
                    err = test_d2nA_dxmdxn[m][s][t].infinityNorm();
                    DGFX_ASSERT( err < MAX_VERIFY_ERROR );

                    if ( DISPLAY ) {
                        std::cout << "test_d2nB_dxmdxn[ " << m << " ][ "
                            << n << " ][ " << s << " ][ " << t << " ] = "
                            << test_d2nB_dxmdxn[ m ][ s ][ t ] << std::endl;
                    }
                    test_d2nB_dxmdxn[m][s][t] -= bv._d2nB_dxmdxn[m][n][s][t];
                    err = test_d2nB_dxmdxn[m][s][t].infinityNorm();
                    DGFX_ASSERT( err < MAX_VERIFY_ERROR );

                    if ( DISPLAY ) {
                        std::cout << "test_d2e_dxmdxn[ " << m << " ][ "
                            << n << " ][ " << s << " ][ " << t << " ] = "
                            << test_d2e_dxmdxn[ m ][ s ][ t ] << std::endl;
                    }
#if DO_NORM
                    test_d2e_dxmdxn[m][s][t] -= bv._d2e_dxmdxn[m][n][s][t];
#endif
                    err = test_d2e_dxmdxn[m][s][t].infinityNorm();
                    DGFX_ASSERT( err < MAX_VERIFY_ERROR );
                }
            }
        }

        DISPLAY = false;
        for( m = 0; m < 4; ++m ) {
            GeVector test_dcosth_dxm, test_dsinth_dxm;
            for( UInt32 s = 0; s < 3; ++s ) {
                const BendVars& testbv = ver_bv[m][s];

                test_dcosth_dxm[ s ] =
                    (testbv._costh - bv._costh ) / VERIFY_STEP;
                test_dsinth_dxm[ s ] =
                    (testbv._sinth - bv._sinth ) / VERIFY_STEP;
            }
            Float err;

            if ( DISPLAY ) {
                std::cout << "test_dcosth_dxm[ " << m << " ] = "
                    << test_dcosth_dxm << std::endl;
            }
            err = (
                test_dcosth_dxm - bv._dcosth_dxm[ m ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR );
            if ( DISPLAY ) {
                std::cout << "test_dsinth_dxm[ " << m << " ] = "
                    << test_dsinth_dxm << std::endl;
            }
            err = (
                test_dsinth_dxm - bv._dsinth_dxm[ m ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR );
        }

        DISPLAY = false;
        for( n = 0; n < 4; ++n ) {
            for( m = 0; m < 4; ++m ) {
                GeMatrix3 test_d2costh_dxmdxn, test_d2sinth_dxmdxn;
                for( t = 0; t < 3; ++t ) {
                    const BendVars& testbv = ver_bv[n][t];
                    const GeVector diffc(
                        ( testbv._dcosth_dxm[ m ] - bv._dcosth_dxm[ m ] ) / VERIFY_STEP
                    );
                    const GeVector diffs(
                        ( testbv._dsinth_dxm[ m ] - bv._dsinth_dxm[ m ] ) / VERIFY_STEP
                    );
                    for ( s = 0; s < 3; ++s ) {
                        test_d2costh_dxmdxn( t, s ) = diffc[ s ];
                        test_d2sinth_dxmdxn( t, s ) = diffs[ s ];
                    }
                }

                Float err;

                if ( DISPLAY ) {
                    std::cout << "test_d2costh_dxmdxn[ " << m << " ][ " << n << " ] = "
                        << test_d2costh_dxmdxn << std::endl;
                }
                err = (
                    test_d2costh_dxmdxn - bv._d2costh_dxmdxn[ m ][ n ]
                ).infinityNorm();
                DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );

                if ( DISPLAY ) {
                    std::cout << "test_d2sinth_dxmdxn[ " << m << " ][ " << n << " ] = "
                        << test_d2sinth_dxmdxn << std::endl;
                }
                err = (
                    test_d2sinth_dxmdxn - bv._d2sinth_dxmdxn[ m ][ n ]
                ).infinityNorm();
                DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
            }
        }

        DISPLAY = false;
        for( m = 0; m < 4; ++m ) {
            GeVector testdC_dxm;
            for( UInt32 s = 0; s < 3; ++s ) {
                const BendVars& testbv = ver_bv[m][s];
                testdC_dxm[ s ] = ( testbv._C - bv._C ) / VERIFY_STEP;
            }
            Float err;
            if ( DISPLAY ) {
                std::cout << "testdC_dxm[ " << m << " ] = "
                    << testdC_dxm << std::endl;
            }
            err = (
                testdC_dxm - bv._dC_dxm[ m ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
        }

        DISPLAY = false;
        for ( n = 0; n < 4; ++n ) for( m = 0; m < 4; ++m ) {
            GeMatrix3 testd2C_dxmdxn;
            for( t = 0; t < 3; ++t ) {
                const BendVars& testbv = ver_bv[n][t];
                const GeVector diff( testbv._dC_dxm[ m ] - bv._dC_dxm[ m ] );
                for( s = 0; s < 3; ++s ) {
                    testd2C_dxmdxn( t, s ) = diff[ s ] / VERIFY_STEP;
                }
            }

            Float err;
            if ( DISPLAY ) {
                std::cout << "testd2C_dxmdxn[ " << m << " ][ " << n
                    << " ] = " << testd2C_dxmdxn << std::endl;
            }
            err = (
                testd2C_dxmdxn - bv._d2C_dxmdxn[ m ][ n ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR_M );
        }
    }
}


////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::CommonVars

//------------------------------------------------------------------------------

void SimSimulator::CommonVars::calc( const GePoint_3 p, const GePoint_3 tp )
{
    _du1 = tp[1]._x - tp[0]._x;
    _dv1 = tp[1]._y - tp[0]._y;
    _du2 = tp[2]._x - tp[0]._x;
    _dv2 = tp[2]._y - tp[0]._y;
    const Float det = _du1 * _dv2 - _du2 * _dv1;
    //DGFX_ASSERT( ! BaMath::isEqual( det, 0 ) );
    const Float detInv = 1 / det;

    // Area in u/v co-ordinates.
    // $ a = \frac{1}{2} \norm{
    //          \begin{pmatrix} \du_1 \\ \dv_1 \\ 0 \end{pmatrix} \cross
    //          \begin{pmatrix} \du_2 \\ \dv_2 \\ 0 \end{pmatrix} } $
    _a = .5 * ( tp[ 1 ] - tp[ 0 ] ).cross( tp[ 2 ] - tp[ 0 ] ).length();

    // $ \whatu = \frac{ (\x_1 - \x_0) \dv_2 - (\x_2 - \x_0) \dv_1}
    //      {\du_1 \dv_2 - \du_2 \dv_1} $
    _wu = ((p[1]-p[0]) * _dv2 + (p[2]-p[0]) * (-_dv1)) * detInv;
    const Float wu_len = _wu.length();
    DGFX_ASSERT( BaMath::isGreater( wu_len, 0 ) );
    _wuim = 1 / wu_len;
    // $ \wu = \frac{ \whatu }{\norm {\whatu}} $
    _whatu = _wu * _wuim;

    // $ \whatv = \frac{ -(\x_1 - \x_0) \du_2 + (\x_2 - \x_0) \du_1}
    //      {\du_1 \dv_2 - \du_2 \dv_1} $
    _wv = (-(p[1]-p[0]) * _du2 + (p[2]-p[0]) * _du1) * detInv;
    const Float wv_len = _wv.length();
    DGFX_ASSERT( BaMath::isGreater( wv_len, 0 ) );
    _wvim = 1 / wv_len;
    // $ \wv = \frac{ \whatv }{\norm {\whatv}} $
    _whatv = _wv * _wvim;

    // Array subscript refers to *which* p this is a derivative
    // of pi, pj or pk. Here, we only have a scalar value - 
    // _dwux_dxmx, the derivative of the x-component of wu with
    // respect to the x-component of pi (or pj, or pk, etc.).
    // dwhu_dxm should be _dwux_dxmx times a 3x3 identity matrix.
    // $ \pfrac{\whatux}{\x_{0_x}} =
    //      \frac{\dv_1 - \dv_2}{\du_1\dv_2 - \du_2\dv_1} $
    // $ \pfrac{\whatux}{\x_{1_x}} = \frac{\dv_2}{\du_1\dv_2 - \du_2\dv_1} $
    // $ \pfrac{\whatux}{\x_{2_x}} = \frac{-\dv_1}{\du_1\dv_2 - \du_2\dv_1} $
    _dwux_dxmx[ 0 ] = (_dv1 - _dv2) * detInv;
    _dwux_dxmx[ 1 ] = _dv2 * detInv;
    _dwux_dxmx[ 2 ] =-_dv1 * detInv;
    // $ \pfrac{\whatvx}{\x_{0_x}} =
    //      \frac{\du_2 - \du_1}{\du_1\dv_2 - \du_2\dv_1} $
    // $ \pfrac{\whatvx}{\x_{1_x}} = \frac{-\du_2}{\du_1\dv_2 - \du_2\dv_1} $
    // $ \pfrac{\whatvx}{\x_{2_x}} = \frac{\du_1}{\du_1\dv_2 - \du_2\dv_1} $
    _dwvx_dxmx[ 0 ] = (_du2 - _du1) * detInv;
    _dwvx_dxmx[ 1 ] =-_du2 * detInv;
    _dwvx_dxmx[ 2 ] = _du1 * detInv;

#if DO_NORM
    UInt32 m,n,s,t;

    for( m = 0; m < 3; ++m ) {
        for ( s = 0; s < 3; ++s ) {
            // $ \pfrac{\wu}{\xms} =
            //      \frac{1}{\norm{\whatu}}
            //      \pfrac{\whatux}{\xmx} \left( \Is - \wus \wu \right) $
            _dwhatu_dxm[ m ][ s ] = _wuim * _dwux_dxmx[ m ] *
                ( GeVector::axis( s ) - _whatu[ s ] * _whatu );
            // $ \pfrac{\wv}{\xms} =
            //      \frac{1}{\norm{\whatv}}
            //      \pfrac{\whatvx}{\xmx} \left( \Is - \wvs \wv \right) $
            _dwhatv_dxm[ m ][ s ] = _wvim * _dwvx_dxmx[ m ] *
                ( GeVector::axis( s ) - _whatv[ s ] * _whatv );
        }
    }

    for( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
        for( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            // $ \pfractwo{\wu}{\xms}{\xnt} &=
            //      -\frac{1}{\norm{\whatu}} \pfrac{\whatux}{\xmx} \left(
            //          \wus \pfrac{\wu}{\xnt} + \pfrac{\wus}{\xnt} \wu
            //      \right) -
            //      \pfrac{\whatux}{\xmx} \pfrac{\whatux}{\xnx}
            //      \frac{( \Is - \wus \wu ) \wut}{\norm{\whatu}^2} $
            _d2whatu_dxmdxn[m][n][s][t] =
                -_wuim * _dwux_dxmx[ m ] *
                (_whatu[s] * _dwhatu_dxm[n][t] + _dwhatu_dxm[n][t][s] * _whatu)
                - _wuim*_wuim * _dwux_dxmx[m] * _dwux_dxmx[n] *
                (GeVector::axis(s) - _whatu[s]*_whatu) * _whatu[t];

            // $ \pfractwo{\wv}{\xms}{\xnt} =
            //    -\frac{1}{\norm{\whatv}} \pfrac{\whatvx}{\xmx} \left(
            //        \wvs \pfrac{\wv}{\xnt} + \pfrac{\wvs}{\xnt} \wv
            //    \right) -
            //    \pfrac{\whatvx}{\xmx} \pfrac{\whatvx}{\xnx}
            //    \frac{( \Is - \wvs \wv ) \wvt}{\norm{\whatv}^2} $
            _d2whatv_dxmdxn[m][n][s][t] =
                -_wvim * _dwvx_dxmx[ m ] *
                (_whatv[s] * _dwhatv_dxm[n][t] + _dwhatv_dxm[n][t][s] * _whatv)
                - _wvim*_wvim * _dwvx_dxmx[m] * _dwvx_dxmx[n] *
                (GeVector::axis(s) - _whatv[s]*_whatv) * _whatv[t];
        }
    }
#endif
}

//------------------------------------------------------------------------------

void SimSimulator::CommonVars::debugOutput( std::ostream& os ) {
    UInt32 m;

    os << "a = " << _a << std::endl;
    os << "whatu = " << _whatu << "  _wuim = " << _wuim << "  wu = "
        << _wu << std::endl;
    os << "whatv = " << _whatv << "  _wvim = " << _wvim << "  wv = "
        << _wv << std::endl;
    for ( m = 0; m < 3; ++m ) {
        os << "_dwux_dxmx[" << m << "] = " << _dwux_dxmx[ m ] << std::endl;
        os << "_dwvx_dxmx[" << m << "] = " << _dwvx_dxmx[ m ] << std::endl;
    }
#if DO_NORM
    UInt32 n,s,t;

    for ( m = 0; m < 3; ++m ) for ( s = 0; s < 3; ++s ) {
        std::cout << "_dwhatu_dxm[" << m << "][" << s << "] = " << _dwhatu_dxm[m][s]
            << std::endl;
        std::cout << "_dwhatv_dxm[" << m << "][" << s << "] = " << _dwhatv_dxm[m][s]
            << std::endl;
    }
    for ( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
        for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            std::cout << "_d2whatu_dxmdxn[" << m << "][" << n << "][" << s << "]["
                << t << "] = " << _d2whatu_dxmdxn[m][n][s][t] << std::endl;
            std::cout << "_d2whatv_dxmdxn[" << m << "][" << n << "][" << s << "]["
                << t << "] = " << _d2whatv_dxmdxn[m][n][s][t] << std::endl;
        }
    }
#endif
}


////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::StretchVars

//------------------------------------------------------------------------------

void SimSimulator::StretchVars::calc(
    const SimSimulator::CommonVars& cv,
    const GeVector v0[ 3 ],
    Float b_u, Float b_v
) {
    UInt32 m,n;
#if DO_SCALE_INVARIANT
    Float alpha = BaMath::sqrt( cv._a );
    // alpha = cv._a ^ 3/4
    alpha = alpha * BaMath::sqrt( alpha );
#else
    const Float alpha = cv._a;
#endif

    // As per [BarWit98] eq. (10)
    // $ \C = a \left(
    //      \begin{pmatrix}
    //          \norm{\wu} - b_u \\ %
    //          \norm{\wv} - b_v
    //      \end{pmatrix} \right) $
    _Cu = alpha * ( cv._wu.length() - b_u );
    _Cv = alpha * ( cv._wv.length() - b_v );

    for ( m = 0; m < 3; ++m ) {
        // $ \pfrac{\Cu}{\xm} = a \pfrac{\whatux}{\xmx} \wu $
        _dCu_dxm[ m ] = alpha * cv._dwux_dxmx[ m ] * cv._whatu;
        // $ \pfrac{\Cv}{\xm} = a \pfrac{\whatvx}{\xmx} \wv $
        _dCv_dxm[ m ] = alpha * cv._dwvx_dxmx[ m ] * cv._whatv;
    }

    // As per [BarWit98] equation just before (11) - but corrected to be
    // a sum.
    _dCu_dt = 0;
    _dCv_dt = 0;
    for( m = 0; m < 3; ++m ) {
        _dCu_dt += _dCu_dxm[ m ].dot( v0[ m ] );
        _dCv_dt += _dCv_dxm[ m ].dot( v0[ m ] );
    }

    // Second derivative of C, split into u and v components
    // FIXME: this is symmetric - if you switch pm and pn, you get the same
    // result.
    for ( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
        // $ \pfractwo{\Cu}{\xm}{\xn} =
        //      \frac{a}{\norm{\wuhat}}
        //      \pfrac{\whatux}{\xmx} \pfrac{\whatux}{\xnx}
        //      \left( \I - \wu \wu^T \right) $

        _d2Cu_dxmdxn[ m ][ n ] =
            alpha * cv._wuim * cv._dwux_dxmx[ m ] * cv._dwux_dxmx[ n ]
            * ( GeMatrix3::IDENTITY -
                GeMatrix3::outerProduct( cv._whatu, cv._whatu ) );

        // $ \pfractwo{\Cv}{\xm}{\xn} =
        //      \frac{a}{\norm{\wvhat}}
        //      \pfrac{\whatvx}{\xmx} \pfrac{\whatvx}{\xnx}
        //      \left( \I - \wv \wv^T \right) $

        _d2Cv_dxmdxn[ m ][ n ] =
            alpha * cv._wvim * cv._dwvx_dxmx[ m ] * cv._dwvx_dxmx[ n ]
            * ( GeMatrix3::IDENTITY -
                GeMatrix3::outerProduct( cv._whatv, cv._whatv ) );
    }
}

//------------------------------------------------------------------------------

void SimSimulator::StretchVars::debugOutput( std::ostream& os )
{
    UInt32 m,n;
    os << "C = ( " << _Cu << ", " << _Cv << " ) " << std::endl;
    for ( m = 0; m < 3; ++m ) {
        os << "dCu_dxm[ " << m << " ] = " << _dCu_dxm[ m ] << std::endl;
        os << "dCv_dxm[ " << m << " ] = " << _dCv_dxm[ m ] << std::endl;
    }
    os << "dCu_dt = " << _dCu_dt << std::endl;
    os << "dCv_dt = " << _dCv_dt << std::endl;
    for ( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
        os << "d2Cu_dxmdxn[ " << m << "," << n << " ] = "
            << _d2Cu_dxmdxn[ m ][ n ] << std::endl;
        os << "d2Cv_dxmdxn[ " << m << "," << n << " ] = "
            << _d2Cv_dxmdxn[ m ][ n ] << std::endl;
    }
}


////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::ShearVars

//------------------------------------------------------------------------------

void SimSimulator::ShearVars::calc(
    const SimSimulator::CommonVars& cv,
    const GeVector v0[ 3 ]
) {
    UInt32 m,n,s;

#if DO_SCALE_INVARIANT
    Float alpha = BaMath::sqrt( cv._a );
    alpha = alpha * BaMath::sqrt( alpha );
#else
    const Float alpha = cv._a;
#endif
    // As per [BarWit98] section 4.3
    // $ C = a \wu \cdot \wv $
#if DO_NORM
    // Correction: use normalised wu, wv
    _C = alpha * cv._whatu.dot( cv._whatv );
#else
    _C = alpha * cv._wu.dot( cv._wv );
#endif

    // $ \pfrac{C}{\xms} = a \left(
    //      \pfrac{\wu}{\xms} \cdot \wv + \wu \cdot pfrac{dwv}{\xms}
    //   \right) $
    for ( m = 0; m < 3; ++m ) {
        for ( s = 0; s < 3; ++s ) {
            _dC_dxm[ m ][ s ] = alpha * (
#if DO_NORM
                cv._dwhatu_dxm[m][s].dot( cv._whatv ) +
                cv._whatu.dot( cv._dwhatv_dxm[m][s] )
#else
                cv._dwux_dxmx[m] * cv._wv[s] +
                cv._wu[s] * cv._dwvx_dxmx[m]
#endif
            );
        }
    }

    // As per [BarWit98] equation just before (11)
    _dC_dt = 0;
    for( m = 0; m < 3; ++m ) {
        _dC_dt += _dC_dxm[ m ].dot( v0[ m ] );
    }

    for ( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
#if DO_NORM
        // $ \pfractwo{C}{\xms}{\xnt} = a \left(
        //      \pfractwo{\wu}{\xms}{\xnt} \cdot \wv +
        //      \pfrac{\wu}{\xms} \cdot \pfrac{\wv}{\xnt} +
        //      \pfrac{\wu}{\xnt} \cdot \pfrac{\wv}{\xms} +
        //      \wu \cdot \pfractwo{\wu}{\xms}{\xnt} \right) $
        for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            _d2C_dxmdxn[m][n]( t, s ) = alpha * (
                cv._d2whatu_dxmdxn[m][n][s][t].dot( cv._whatv ) +
                cv._dwhatu_dxm[m][s].dot( cv._dwhatv_dxm[n][t] ) +
                cv._dwhatu_dxm[n][t].dot( cv._dwhatv_dxm[m][s] ) +
                cv._whatu.dot( cv._d2whatv_dxmdxn[m][n][s][t] )
            );
        }
#else
        // $ \pfractwo{C}{\xms}{\xnt} = a \left(
        //      \pfrac{\whatu}{\xms} \cdot \pfrac{\whatv}{\xnt} +
        //      \pfrac{\whatu}{\xnt} \cdot \pfrac{\whatv}{\xms} \right) $
        _d2C_dxmdxn[m][n] = GeMatrix3::ZERO;
        for ( s = 0; s < 3; ++s ) {
            _d2C_dxmdxn[m][n]( s, s ) = alpha * (
                cv._dwux_dxmx[m] * cv._dwvx_dxmx[n] +
                cv._dwux_dxmx[n] * cv._dwvx_dxmx[m]
            );
        }
#endif
    }
}

//------------------------------------------------------------------------------

void SimSimulator::ShearVars::debugOutput( std::ostream& os )
{
    UInt32 m,n;

    os << "C = " << _C << std::endl;
    for ( m = 0; m < 3; ++m ) {
        os << "dC_dxm[ " << m << " ] = " << _dC_dxm[ m ] << std::endl;;
    }
    os << "dC_dt = " << _dC_dt << std::endl;;
    for ( m = 0; m < 3; ++m ) for ( n = 0; n < 3; ++n ) {
        os << "d2C_dxmdxn[ " << m << "," << n << " ] = "
            << _d2C_dxmdxn[ m ][ n ] << std::endl;
    }
}


////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::BendVars

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calc(
    const GePoint_4 x,
    const GeVector_4 v
) {
    // My x[0],x[1],x[2],x[3] are equivalent to Macri's ph,pi,pj,pk
    // My nhatA,nhatB are equivalent to Baraff & Witkin's n1,n2
    // i.e. unit vectors
    // My nA, nB are unscaled versions of nhatA,nhatB
    // _nAim = 1 / || nA ||      "nA inverse magnitude"
    calcNE( x );
    mainCalc( x, v );
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calc(
    const GePoint_4 x,
    const GeVector_4 v,
    const BendVars& bv
) {
    calcNE( x );
#if !DO_NORM
    // Enforce the assumption that the normals have constant magnitude - don't
    // scale by 1/||nA||, instead scale by the inverse of the *old* normal's
    // magnitude.
    _nhatA *= bv._nAim / _nAim;
    _nAim = bv._nAim;
    _nhatB *= bv._nBim / _nBim;
    _nBim = bv._nBim;
    _ehat *= bv._eim / _eim;
    _eim = bv._eim;
#endif
    mainCalc( x, v );
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::mainCalc(
    const GePoint_4 x,
    const GeVector_4 v0
) {
    UInt32 m;

    calcThetas();
    calcQs( x );
    calcDqs();
    calcNEderiv();
    calcD2NE();
    calcCosSinDeriv();
    calcD2cossinth();
    calcCderivs();

    _dC_dt = 0;
    // As per [BarWit98] equation just before (11)
    for( m = 0; m < 4; ++m ) {
        _dC_dt += _dC_dxm[ m ].dot( v0[ m ] );
    }
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcNE( const GePoint_4 x )
{
    // $ \nhat^A = (\x_2 - \x_0) \cross (\x_1 - \x_0) $
    const GeVector nA( (x[2] - x[0]).cross( x[1] - x[0] ) );
    // nA inverse magnitude
    _nAim = 1 / nA.length();
    // $ \n^A = \frac{\nhat^A}{\norm{\nhat^A}} $
    _nhatA = _nAim * nA;

    // $ \nhat^B = (\x_1 - \x_3) \cross (\x_2 - \x_3) $
    const GeVector nB( (x[1] - x[3]).cross( x[2] - x[3] ) );
    _nBim = 1 / nB.length();
    // $ \n^B = \frac{\nhat^B}{\norm{\nhat^B}} $
    _nhatB = _nBim * nB;

    // $ \ehat = \x_1 - \x_2 $
    const GeVector e( x[1] - x[2] );
    _eim = 1 / e.length();
    // $ \e = \frac{\ehat}{\norm{\ehat}} $
    _ehat = _eim * e;
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcThetas()
{
    // $ \cos \theta = \n^A \cdot \n^B $
    _costh = _nhatA.dot( _nhatB );
    // $ \sin \theta = (\n^A \cross \n^B) \cdot \e $
    _sinth = _nhatA.cross( _nhatB ).dot( _ehat );
    // FIXME: not *just* arctan
    // $ C = \theta = \arctan \frac{\sin \theta}{\cos \theta} $
    // In range [-pi,+pi] - Macri uses abs of this!
    _C = BaMath::arctan2( _sinth, _costh );
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcQs( const GePoint_4 x )
{
    // $ \pfrac{\nhat^A}{\xms} = S_s(\q^A_m) $
    // $ \q^A_0 = \x_2 - \x_1 $
    // $ \q^A_1 = \x_0 - \x_2 $
    // $ \q^A_2 = \x_1 - \x_0 $
    // $ \q^A_3 = {\bf 0} $
    _qA[ 0 ] = x[2] - x[1];
    _qA[ 1 ] = x[0] - x[2];
    _qA[ 2 ] = x[1] - x[0];
    _qA[ 3 ] = GeVector::ZERO;

    // $ \pfrac{\nhat^B}{\xms} = S_s(\q^B_m) $
    // $ \q^B_0 = {\bf 0} $
    // $ \q^B_1 = \x_2 - \x_3 $
    // $ \q^B_2 = \x_3 - \x_1 $
    // $ \q^B_3 = \x_1 - \x_2 $
    _qB[ 0 ] = GeVector::ZERO;
    _qB[ 1 ] = x[2] - x[3];
    _qB[ 2 ] = x[3] - x[1];
    _qB[ 3 ] = x[1] - x[2];

    // $ \pfrac{\ehat}{\xms} = q^e_m \Is $
    // $ q^e = \begin{pmatrix} 0 \\ 1 \\ -1 \\ 0 \end{pmatrix} $
    _qe[ 0 ] = 0;
    _qe[ 1 ] = 1;
    _qe[ 2 ] = -1;
    _qe[ 3 ] = 0;
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcDqs()
{
    // FIXME: document this
    
    // Row represents m, column represents n
    const Int32 D1[4][4] = {
        { 0, -1, 1, 0 },
        { 1, 0, -1, 0 },
        { -1, 1, 0, 0 },
        { 0, 0, 0, 0 }
    };
    const Int32 D2[4][4] = {
        { 0, 0, 0, 0 },
        { 0, 0, 1, -1 },
        { 0, -1, 0, 1 },
        { 0, 1, -1, 0 }
    };
    for( UInt32 m = 0; m < 4; ++m ) {
        for( UInt32 n = 0; n < 4; ++n ) {
            for( UInt32 t = 0; t < 3; ++t ) {
                _dqAm_dxn[m][n][t] = GeVector::ZERO;
                _dqAm_dxn[m][n][t][t] = D1[m][n];
                _dqBm_dxn[m][n][t] = GeVector::ZERO;
                _dqBm_dxn[m][n][t][t] = D2[m][n];
            }
        }
    }
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcNEderiv()
{
#if DO_NORM
    const GeMatrix3 InAnAt =
        GeMatrix3::IDENTITY - GeMatrix3::outerProduct( _nhatA, _nhatA );
    const GeMatrix3 InBnBt =
        GeMatrix3::IDENTITY - GeMatrix3::outerProduct( _nhatB, _nhatB );
    const GeMatrix3 Ieet =
        GeMatrix3::IDENTITY - GeMatrix3::outerProduct( _ehat, _ehat );

    for ( UInt32 m = 0; m < 4; ++m ) {
        for ( UInt32 s = 0; s < 3; ++s ) {
            // $ \pfrac{\n^A}{\xms} = \left(\I - \n^A(\n^A)^T\right)
            //      \frac{ S_s( \q^A_m ) }{ \norm{\nhat^A} } $
            _dnhatA_dxm[ m ][ s ] = InAnAt * makeSkewRow( _qA[ m ], s ) * _nAim;
            // $ \pfrac{\n^B}{\xms} = \left(\I - \n^B(\n^B)^T\right)
            //      \frac{ S_s( \q^B_m ) }{ \norm{\nhat^B} } $
            _dnhatB_dxm[ m ][ s ] = InBnBt * makeSkewRow( qB[ m ], s ) * _nBim;
            // $ \pfrac{\e}{\xms} = (\I - \e\e^T)
            //      \frac{ \Is( q^e_m ) }{ \norm{\ehat} } $
            _dehat_dxm[ m ][ s ] = Ieet * GeVector::axis( s ) * _qe[ m ] * _eim;
        }
    }
#else
    for ( UInt32 m = 0; m < 4; ++m ) {
        for ( UInt32 s = 0; s < 3; ++s ) {
            _dnhatA_dxm[ m ][ s ] = makeSkewRow( _qA[ m ], s ) * _nAim;
            _dnhatB_dxm[ m ][ s ] = makeSkewRow( _qB[ m ], s ) * _nAim;
            _dehat_dxm[ m ][ s ] = GeVector::axis( s ) * _qe[ m ] * _eim;
        }
    }
#endif
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcD2NE()
{
    UInt32 m,n,s,t;
#if DO_NORM
    for ( m = 0; m < 4; ++m ) for ( n = 0; n < 4; ++n ) {
        for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            GeMatrix3 tempm;
            GeVector tempv;

            // $ \pfractwo{\n^A}{\xms}{\xnt} =
            // - \left(
            //      \pfrac{\n^A}{\xnt} (\n^A)^T +
            //      \n^A \left( \pfrac{\n^A}{\xnt} \right)^T
            //   \right) \frac{S_s(\q^A_m)}{\norm{\nhat^A}} +
            //   \frac{\I - \n^A(\n^A)^T}{\norm{\nhat^A}^2}
            //   \left(
            //      \norm{\nhat^A} S_s\left( \pfrac{\q^A_m}{\xnt} \right) -
            //      \left( S_t(\q^A_n) \cdot \n^A \right) S_s(\q^A_m)
            //   \right) $

            GeVector SqAm = makeSkewRow( _qA[ m ], s );
            GeVector SqAn = makeSkewRow( _qA[ n ], t );
            tempm = GeMatrix3::outerProduct( _dnhatA_dxm[ n ][ t ], _nhatA );
            tempm += GeMatrix3::outerProduct( _nhatA, _dnhatA_dxm[ n ][ t ] );
            _d2nA_dxmdxn[m][n][s][t] = tempm * SqAm * -_nAim;
            tempm = (
                GeMatrix3::IDENTITY - GeMatrix3::outerProduct( _nhatA, _nhatA )
            ) * (_nAim * _nAim);
            tempv = makeSkewRow( _dqAm_dxn[ m ][ n ][ t ], s ) / _nAim;
            tempv -= SqAn.dot( _nhatA ) * SqAm;
            _d2nA_dxmdxn[m][n][s][t] += tempm * tempv;

            // $ \pfractwo{\n^B}{\xms}{\xnt} =
            // - \left(
            //      \pfrac{\n^B}{\xnt} (\n^B)^T +
            //      \n^B \left( \pfrac{\n^B}{\xnt} \right)^T
            //   \right) \frac{S_s(\q^B_m)}{\norm{\nhat^B}} +
            //   \frac{\I - \n^B(\n^B)^T}{\norm{\nhat^B}^2}
            //   \left(
            //      \norm{\nhat^B} S_s\left( \pfrac{\q^B_m}{\xnt} \right) -
            //      \left( S_t(\q^B_n) \cdot \n^B \right) S_s(\q^B_m)
            //   \right) $

            GeVector SqBm = makeSkewRow( qB[ m ], s );
            GeVector SqBn = makeSkewRow( qB[ n ], t );
            tempm = GeMatrix3::outerProduct( _dnhatB_dxm[ n ][ t ], _nhatB );
            tempm += GeMatrix3::outerProduct( _nhatB, _dnhatB_dxm[ n ][ t ] );
            _d2nB_dxmdxn[m][n][s][t] = tempm * SqBm * -_nBim;
            tempm = (
                GeMatrix3::IDENTITY -GeMatrix3::outerProduct( _nhatB, _nhatB )
            ) * (_nBim * _nBim);
            tempv = makeSkewRow( _dqBm_dxn[ m ][ n ][ t ], s ) / _nBim;
            tempv -= SqBn.dot( _nhatB ) * SqBm;
            _d2nB_dxmdxn[m][n][s][t] += tempm * tempv;

            // $ \pfractwo{\e}{\xms}{\xnt} =
            // - \left(
            //      \pfrac{\e}{\xnt} \e^T +
            //      \e \left( \pfrac{\e}{\xnt} \right)^T
            //   \right) \frac{S_s(q^e_m)}{\norm{\ehat}} -
            //   \frac{\I - \e\e^T}{\norm{\ehat}^2}
            //      q^e_m q^e_n \e_t \Is $

            tempm = GeMatrix3::outerProduct( _dehat_dxm[ n ][ t ], _ehat );
            tempm += GeMatrix3::outerProduct( _ehat, _dehat_dxm[ n ][ t ] );
            _d2e_dxmdxn[m][n][s][t] = tempm * GeVector::axis( s ) *
                (-_qe[ m ] * _eim);
            tempm =
                GeMatrix3::IDENTITY - GeMatrix3::outerProduct( _ehat, _ehat );
            tempv = _eim * _eim * _qe[ n ] * _qe[ m ] * _ehat[ t ] *
                GeVector::axis( s );
            _d2e_dxmdxn[m][n][s][t] -= tempm * tempv;
        }
    }
#else
    for ( m = 0; m < 4; ++m ) for ( n = 0; n < 4; ++n ) {
        for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            _d2nA_dxmdxn[m][n][s][t] =
                makeSkewRow( _dqAm_dxn[m][n][t], s ) * _nAim;
            _d2nB_dxmdxn[m][n][s][t] =
                makeSkewRow( _dqBm_dxn[m][n][t], s ) * _nBim;
        }
    }
#endif
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcCosSinDeriv()
{
    UInt32 m,s;

    // $ \pfrac{ \cos \theta }{\xms} =
    //   \pfrac{ \n^A \cdot \n^B }{\xms} =
    //      \pfrac{\n^A}{\xms} \cdot \n^B +
    //      \n^A \cdot \pfrac{\n^B}{\xms} $
    for( m = 0; m < 4; ++m ) {
        for ( s = 0; s < 3; ++s ) {
            _dcosth_dxm[m][s] =
                _dnhatA_dxm[m][s].dot( _nhatB ) +
                _nhatA.dot( _dnhatB_dxm[m][s] );
        }
    }

    // $ \pfrac{ \sin \theta }{\xms} =
    //   \pfrac{ (\n^A \cross \n^B) \cdot \e}{\xms} =
    //      \left(
    //          \pfrac{\n^A}{\xms} \cross \n^B +
    //          \n^A \cross \pfrac{\n^B}{\xms}
    //      \right) \cdot \e +
    //      (\n^A \cross \n^B) \cdot \pfrac{\e}{\xms} $
    GeVector nhatAxnhatB( _nhatA.cross( _nhatB ) );
    for( m = 0; m < 4; ++m ) {
        for ( s = 0; s < 3; ++s ) {
            _dsinth_dxm[m][s] =
                (
                    _dnhatA_dxm[m][s].cross( _nhatB ) +
                    _nhatA.cross( _dnhatB_dxm[m][s] )
                ).dot( _ehat ) + nhatAxnhatB.dot( _dehat_dxm[m][s] );
        }
    }
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcD2cossinth()
{
    UInt32 m,n,s,t;

    // $ \pfractwo{\cos \theta}{\xmt}{\xns} =
    //      \pfractwo{\n^A}{\xms}{\xnt} \cdot \n^B +
    //      \pfrac{\n^B}{\xnt} \cdot \pfrac{\n^A}{\xms} +
    //      \pfrac{\n^A}{\xnt} \cdot \pfrac{\n^B}{\xms} +
    //      \n^A \cdot \pfractwo{\n^B}{\xms}{\xnt} $
    for( m = 0; m < 4; ++m ) for( n = 0; n < 4; ++n ) {
        for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            _d2costh_dxmdxn[m][n](t,s) =
                _d2nA_dxmdxn[m][n][s][t].dot( _nhatB ) +
                _dnhatA_dxm[m][s].dot( _dnhatB_dxm[n][t] ) +
                _dnhatA_dxm[n][t].dot( _dnhatB_dxm[m][s] ) +
                _d2nB_dxmdxn[m][n][s][t].dot( _nhatA );
        }
    }

    // $ \pfractwo{\sin \theta}{\xms}{\xnt} =
    //      \left(
    //          \pfractwo{\n^A}{\xms}{\xnt} \cross \n^B +
    //          \pfrac{\n^A}{\xms} \cross \pfrac{\n^B}{\xnt} +
    //          \pfrac{\n^A}{\xnt} \cross \pfrac{\n^B}{\xms} +
    //          \n^A \cross \pfractwo{\n^B}{\xms}{\xnt}
    //      \right) \cdot \e +
    //      \left(
    //          \pfrac{\n^A}{\xms} \cross \n^B +
    //          \n^A \cross \pfrac{\n^B}{\xms}
    //      \right) \cdot \pfrac{\e}{\xnt} +
    //      \left(
    //          \pfrac{\n^A}{\xnt} \cross \n^B +
    //          \n^A \cross \pfrac{\n^B}{\xnt}
    //      \right) \cdot \pfrac{\e}{\xms} +
    //      (\n^A \cross \n^B) \pfractwo{\e}{\xms}{\xnt} $
    for( m = 0; m < 4; ++m ) for( n = 0; n < 4; ++n ) {
        for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            _d2sinth_dxmdxn[ m ][ n ]( t, s ) = (
                _d2nA_dxmdxn[m][n][s][t].cross( _nhatB ) +
                _dnhatA_dxm[m][s].cross( _dnhatB_dxm[n][t] ) +
                _dnhatA_dxm[n][t].cross( _dnhatB_dxm[m][s] ) +
                _nhatA.cross( _d2nB_dxmdxn[m][n][s][t] )
            ).dot( _ehat );
            _d2sinth_dxmdxn[ m ][ n ]( t, s ) += (
                _dnhatA_dxm[m][s].cross( _nhatB ) +
                _nhatA.cross( _dnhatB_dxm[m][s] )
            ).dot( _dehat_dxm[n][t] );
            _d2sinth_dxmdxn[ m ][ n ]( t, s ) += (
                _dnhatA_dxm[n][t].cross( _nhatB ) +
                _nhatA.cross( _dnhatB_dxm[n][t] )
            ).dot( _dehat_dxm[m][s] );
#if DO_NORM
            _d2sinth_dxmdxn[ m ][ n ]( t, s ) +=
                _nhatA.cross( _nhatB ).dot( _d2e_dxmdxn[m][n][s][t] );
#endif
        }
    }
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::calcCderivs()
{
    UInt32 m,n,s,t;

    // $ \pfrac{C}{\xms} = \pfrac{\theta}{\xms} =
    //      \cos \theta \pfrac{\sin \theta}{\xms} -
    //      \sin \theta \pfrac{\cos \theta}{\xms} $
    for ( m = 0; m < 4; ++m ) {
        _dC_dxm[ m ] = _costh * _dsinth_dxm[ m ] - _sinth * _dcosth_dxm[ m ];
    }
    // $ \pfractwo{C}{\xms}{\xnt} = \pfractwo{\theta}{\xms}{\xnt} =
    //      \pfrac{\cos \theta}{\xnt}\pfrac{\sin \theta}{\xms} +
    //      \cos \theta \pfractwo{\sin \theta}{\xms}{\xnt} -
    //      \pfrac{\sin \theta}{\xnt} \pfrac{\cos \theta}{\xms} -
    //      \sin \theta \pfractwo{\cos \theta}{\xms}{\xnt} $
    for ( m = 0; m < 4; ++m ) for ( n = 0; n < 4; ++n ) {
        for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            _d2C_dxmdxn[m][n](t,s) =
                _dcosth_dxm[n][t] * _dsinth_dxm[m][s] +
                _costh * _d2sinth_dxmdxn[m][n](t,s) -
                _dsinth_dxm[n][t] * _dcosth_dxm[m][s] -
                _sinth * _d2costh_dxmdxn[m][n](t,s);
        }
    }
            
}

//------------------------------------------------------------------------------

void SimSimulator::BendVars::debugOutput( std::ostream& os )
{
    UInt32 m,n,s,t;

    os << "nhatA = " << _nhatA << "  _nAim = " << _nAim << std::endl;
    os << "nhatB = " << _nhatB << "  _nBim = " << _nBim << std::endl;
    os << "ehat = " << _ehat << "  _eim = " << _eim << std::endl;

    os  << "cos(theta) = " << _costh << "  sin(theta) = " << _sinth
        << "  C = theta = " << _C << " radians ("
        << _C * 180 / M_PI << " degrees)"
        << std::endl;

    for ( m = 0; m < 4; ++m ) for ( n = 0; n < 4; ++n ) {
        for ( s = 0; s < 3; ++s ) {
            os << "dqAm_dxn[ " << m << " ][ " << n << " ][ "
                << s << " ] = " << _dqAm_dxn[m][n][s] << std::endl;
            os << "dqBm_dxn[ " << m << " ][ " << n << " ][ "
                << s << " ] = " << _dqBm_dxn[m][n][s] << std::endl;
        }
    }

    for( m = 0; m < 4; ++m ) {
        for( s = 0; s < 3; ++s ) {
            os << "dnhatA_dxm[ " << m << " ][ " << s << " ] = "
                << _dnhatA_dxm[ m ][ s ] << std::endl;
            os << "dnhatB_dxm[ " << m << " ][ " << s << " ] = "
                << _dnhatB_dxm[ m ][ s ] << std::endl;
            os << "dehat_dxm[ " << m << " ][ " << s << " ] = "
                << _dehat_dxm[ m ][ s ] << std::endl;
        }
    }

    for ( m = 0; m < 4; ++m ) for ( n = 0; n < 4; ++n ) {
        for ( s = 0; s < 3; ++s ) for ( t = 0; t < 3; ++t ) {
            os << "d2nA_dxmdxn[ " << m << " ][ " << n << " ][ "
                << s << " ][ " << t << " ] = " << _d2nA_dxmdxn[m][n][s][t]
                << std::endl;
            os << "d2nB_dxmdxn[ " << m << " ][ " << n << " ][ "
                << s << " ][ " << t << " ] = " << _d2nB_dxmdxn[m][n][s][t]
                << std::endl;
#if DO_NORM
            os << "d2e_dxmdxn[ " << m << " ][ " << n << " ][ "
                << s << " ][ " << t << " ] = " << _d2e_dxmdxn[m][n][s][t]
                << std::endl;
#endif
        }
    }

    for ( m = 0; m < 4; ++m ) {
        os << "dcosth_dxm[ " << m << " ] = " << _dcosth_dxm[ m ] << std::endl;
        os << "dsinth_dxm[ " << m << " ] = " << _dsinth_dxm[ m ] << std::endl;
    }

    for( m = 0; m < 4; ++m ) for( n = 0; n < 4; ++n ) {
        os << "d2costh_dxmdxn[ " << m << " ][ " << n << " ] = "
            << _d2costh_dxmdxn[ m ][ n ] << std::endl;
        os << "d2sinth_dxmdxn[ " << m << " ][ " << n << " ] = "
            << _d2sinth_dxmdxn[ m ][ n ] << std::endl;
    }

    for ( m = 0; m < 4; ++m ) {
        os << "dC_dxm[ " << m << " ] = " << _dC_dxm[ m ] << std::endl;;
    }
    os << "dC_dt = " << _dC_dt << std::endl;
    for ( m = 0; m < 4; ++m ) for ( n = 0; n < 4; ++n ) {
        os << "d2C_dxmdxn[ " << m << "," << n << " ] = "
            << _d2C_dxmdxn[ m ][ n ] << std::endl;
    }
}


////////////////////////////////////////////////////////////////////////////////
// CLASS SimSimulator::ModPCGSolver
//

//------------------------------------------------------------------------------

SimSimulator::ModPCGSolver::ModPCGSolver()
 : _tolerance( 1e-4f )
{
}

//------------------------------------------------------------------------------

bool SimSimulator::ModPCGSolver::done() const
{
    return _done;
}

//------------------------------------------------------------------------------

const SimVector& SimSimulator::ModPCGSolver::result() const
{
    return _x;
}

//------------------------------------------------------------------------------

void SimSimulator::ModPCGSolver::setTolerance( Float tolerance )
{
    _tolerance = tolerance;
}

//------------------------------------------------------------------------------

Float SimSimulator::ModPCGSolver::getTolerance() const
{
    return _tolerance;
}

//------------------------------------------------------------------------------

void SimSimulator::ModPCGSolver::preStep()
{
    setupPreconditioner();
    setupCG();
    _nbSteps = 0;
    _done = false;
}

//------------------------------------------------------------------------------

void SimSimulator::ModPCGSolver::setupPreconditioner()
{
    // FIXME: this assertion is failing... why?
    //DGFX_ASSERT( isSymmetric( _A ) );

    UInt32 N = _A.nbRows();
    if ( _P.nbRows() != _A.nbRows() ) {
        _P = SimMatrix( N, N );
        _Pinv = SimMatrix( N, N );

    }
    for ( UInt32 i = 0; i < N; ++i ) {
        const GeMatrix3& a = _A( i, i );
        GeMatrix3& p = _P( i, i );
        GeMatrix3& pinv = _Pinv( i, i );
        p = GeMatrix3::ZERO;
        pinv = GeMatrix3::ZERO;
        for ( UInt32 j = 0; j < 3; ++j ) {
            DGFX_ASSERT( !BaMath::isEqual( a( j, j ), 0, 10e-12f ) );
            // This is the inverse of the [BarWit98]'s recommendation...
            // but makes things work much better.
            p( j, j ) = a( j, j );
            pinv( j, j ) = 1 / a( j, j );
        }
    }
}

//------------------------------------------------------------------------------

void SimSimulator::ModPCGSolver::setupCG()
{
    _x = _z;
    const SimVector bf( filter( _b ) );
    _delta0 = ( _P * bf ).dot( bf );
    _r = filter( _b - _A * _x );
    _c = filter( _Pinv * _r );
    _deltaNew = _r.dot( _c );
}
    
//------------------------------------------------------------------------------

SimVector SimSimulator::ModPCGSolver::filter( const SimVector& a ) const
{
    DGFX_ASSERT( _S.size() == a.size() );
    SimVector result( a.size() );
    std::vector<GeMatrix3>::const_iterator Si = _S.begin();
    SimVector::const_iterator ai = a.begin();
    SimVector::iterator ri = result.begin();
    for( ; Si != _S.end(); ++Si, ++ai, ++ri ) {
        *ri = *Si * *ai;
    }
    return result;
}

//------------------------------------------------------------------------------

void SimSimulator::ModPCGSolver::filterInPlace( SimVector& a ) const
{
    DGFX_ASSERT( _S.size() == a.size() );
    std::vector<GeMatrix3>::const_iterator Si = _S.begin();
    SimVector::iterator ai = a.begin();
    for( ; Si != _S.end(); ++Si, ++ai ) {
        *ai = *Si * *ai;
    }
}

//------------------------------------------------------------------------------

void SimSimulator::ModPCGSolver::step()
{
    if ( _deltaNew < _tolerance * _tolerance * _delta0 ) {
        _done = true;
        return;
    }

    SimMatrix::multiply( _q, _A, _c );
    filterInPlace( _q );
    Float alpha = _deltaNew / _c.dot( _q );
    _x.plusEqualsScaled( alpha, _c );
    _r.plusEqualsScaled( -alpha, _q );

    SimMatrix::multiply( _s, _Pinv, _r );
    Float _deltaOld = _deltaNew;
    _deltaNew = _r.dot( _s );
    _c *= _deltaNew / _deltaOld;
    _c += _s;
    filterInPlace( _c );
    ++_nbSteps;
}

FREECLOTH_NAMESPACE_END

---