---

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;
    GeVector wuhat,wvhat;
    Float wuhim,wvhim;

    Float_3 dwhux_dxmx, dwhvx_dxmx;
#if DO_NORM
    GeVector_33 dwu_dxm, dwv_dxm;
    GeVector_3333 d2wu_dxmdxn, d2wv_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 -----
    GeVector nA,nB,e;
    Float nAhim,nBhim,ehim;
    Float costh,sinth;
    Float C;

    GeVector_4 qA, qB;
    Float_4 qe;
    GeVector_443 dqAm_dxn, dqBm_dxn;

    GeVector_43 dnA_dxm, dnB_dxm, de_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 testdwu_dxm = ( testcv.wu - cv.wu ) / VERIFY_STEP;
            GeVector testdwv_dxm = ( testcv.wv - cv.wv ) / VERIFY_STEP;

            Float err;

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

            if ( DISPLAY ) {
                std::cout << "testdwv_dxm[ " << m << " ][ " << s << " ] = "
                    << testdwv_dxm << std::endl;
            }
            err = ( testdwv_dxm - cv.dwv_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 testd2wu_dxmdxn =
                    (testcv.dwu_dxm[m][s] - cv.dwu_dxm[m][s]) / VERIFY_STEP;
                GeVector testd2wv_dxmdxn =
                    (testcv.dwv_dxm[m][s] - cv.dwv_dxm[m][s]) / VERIFY_STEP;

                Float err;

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

                if ( DISPLAY ) {
                    std::cout << "testd2wv_dxmdxn[" << m << "][" << n << "]["
                        << s << "][" << t << "] = "
                        << testd2wv_dxmdxn << std::endl;
                }
                err = (
                    testd2wv_dxmdxn - cv.d2wv_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 testdnA_dxm = ( testbv.nA - bv.nA ) / VERIFY_STEP;
            GeVector testdnB_dxm = ( testbv.nB - bv.nB ) / VERIFY_STEP;
            GeVector testde_dxm = ( testbv.e - bv.e ) / VERIFY_STEP;

            Float err;

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

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

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

        DISPLAY = false;
        for( n = 0; n < 4; ++n ) {
            GeVector testd2nA_dxmdxn[ 4 ][ 3 ][ 3 ];
            GeVector testd2nB_dxmdxn[ 4 ][ 3 ][ 3 ];
            GeVector testd2e_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 ) {
                    testd2nA_dxmdxn[ m ][ s ][ t ] =
                        ( testbv.dnA_dxm[ m ][ s ] - bv.dnA_dxm[ m ][ s ] ) /
                        VERIFY_STEP;
                    testd2nB_dxmdxn[ m ][ s ][ t ] =
                        ( testbv.dnB_dxm[ m ][ s ] - bv.dnB_dxm[ m ][ s ] ) /
                        VERIFY_STEP;
                    testd2e_dxmdxn[ m ][ s ][ t ] =
                        ( testbv.de_dxm[ m ][ s ] - bv.de_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 << "testd2nA_dxmdxn[ " << m << " ][ "
                            << n << " ][ " << s << " ][ " << t << " ] = "
                            << testd2nA_dxmdxn[ m ][ s ][ t ] << std::endl;
                    }
                    testd2nA_dxmdxn[m][s][t] -= bv.d2nA_dxmdxn[m][n][s][t];
                    err = testd2nA_dxmdxn[m][s][t].infinityNorm();
                    DGFX_ASSERT( err < MAX_VERIFY_ERROR );

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

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

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

                testdcosth_dxm[ s ] = (testbv.costh - bv.costh ) / VERIFY_STEP;
                testdsinth_dxm[ s ] = (testbv.sinth - bv.sinth ) / VERIFY_STEP;
            }
            Float err;

            if ( DISPLAY ) {
                std::cout << "testdcosth_dxm[ " << m << " ] = "
                    << testdcosth_dxm << std::endl;
            }
            err = (
                testdcosth_dxm - bv.dcosth_dxm[ m ]
            ).infinityNorm();
            DGFX_ASSERT( err < MAX_VERIFY_ERROR );
            if ( DISPLAY ) {
                std::cout << "testdsinth_dxm[ " << m << " ] = "
                    << testdsinth_dxm << std::endl;
            }
            err = (
                testdsinth_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 testd2costh_dxmdxn, testd2sinth_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 ) {
                        testd2costh_dxmdxn( t, s ) = diffc[ s ];
                        testd2sinth_dxmdxn( t, s ) = diffs[ s ];
                    }
                }

                Float err;

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

                if ( DISPLAY ) {
                    std::cout << "testd2sinth_dxmdxn[ " << m << " ][ " << n << " ] = "
                        << testd2sinth_dxmdxn << std::endl;
                }
                err = (
                    testd2sinth_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 det_inv = 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} $
    wuhat = ((p[1]-p[0]) * dv2 + (p[2]-p[0]) * (-dv1)) * det_inv;
    const Float wuh_len = wuhat.length();
    DGFX_ASSERT( BaMath::isGreater( wuh_len, 0 ) );
    wuhim = 1 / wuh_len;
    // $ \wu = \frac{ \whatu }{\norm {\whatu}} $
    wu = wuhat * wuhim;

    // $ \whatv = \frac{ -(\x_1 - \x_0) \du_2 + (\x_2 - \x_0) \du_1}
    //      {\du_1 \dv_2 - \du_2 \dv_1} $
    wvhat = (-(p[1]-p[0]) * du2 + (p[2]-p[0]) * du1) * det_inv;
    const Float wvh_len = wvhat.length();
    DGFX_ASSERT( BaMath::isGreater( wvh_len, 0 ) );
    wvhim = 1 / wvh_len;
    // $ \wv = \frac{ \whatv }{\norm {\whatv}} $
    wv = wvhat * wvhim;

    // Array subscript refers to *which* p this is a derivative
    // of pi, pj or pk. Here, we only have a scalar value - 
    // dwhux_dxmx, the derivative of the x-component of wuhat with
    // respect to the x-component of pi (or pj, or pk, etc.).
    // dwhu_dxm should be dwhux_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} $
    dwhux_dxmx[ 0 ] = (dv1 - dv2) * det_inv;
    dwhux_dxmx[ 1 ] = dv2 * det_inv;
    dwhux_dxmx[ 2 ] =-dv1 * det_inv;
    // $ \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} $
    dwhvx_dxmx[ 0 ] = (du2 - du1) * det_inv;
    dwhvx_dxmx[ 1 ] =-du2 * det_inv;
    dwhvx_dxmx[ 2 ] = du1 * det_inv;

#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) $
            dwu_dxm[ m ][ s ] = wuhim * dwhux_dxmx[ m ] *
                ( GeVector::axis( s ) - wu[ s ] * wu );
            // $ \pfrac{\wv}{\xms} =
            //      \frac{1}{\norm{\whatv}}
            //      \pfrac{\whatvx}{\xmx} \left( \Is - \wvs \wv \right) $
            dwv_dxm[ m ][ s ] = wvhim * dwhvx_dxmx[ m ] *
                ( GeVector::axis( s ) - wv[ s ] * wv );
        }
    }

    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} $
            d2wu_dxmdxn[m][n][s][t] =
                -wuhim * dwhux_dxmx[ m ] *
                (wu[s] * dwu_dxm[n][t] + dwu_dxm[n][t][s] * wu)
                - wuhim*wuhim * dwhux_dxmx[m] * dwhux_dxmx[n] *
                (GeVector::axis(s) - wu[s]*wu) * wu[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} $
            d2wv_dxmdxn[m][n][s][t] =
                -wvhim * dwhvx_dxmx[ m ] *
                (wv[s] * dwv_dxm[n][t] + dwv_dxm[n][t][s] * wv)
                - wvhim*wvhim * dwhvx_dxmx[m] * dwhvx_dxmx[n] *
                (GeVector::axis(s) - wv[s]*wv) * wv[t];
        }
    }
#endif
}

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

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

    os << "a = " << a << std::endl;
    os << "wuhat = " << wuhat << "  wuhim = " << wuhim << "  wu = "
        << wu << std::endl;
    os << "wvhat = " << wvhat << "  wvhim = " << wvhim << "  wv = "
        << wv << std::endl;
    for ( m = 0; m < 3; ++m ) {
        os << "dwhux_dxmx[" << m << "] = " << dwhux_dxmx[ m ] << std::endl;
        os << "dwhvx_dxmx[" << m << "] = " << dwhvx_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 << "dwu_dxm[" << m << "][" << s << "] = " << dwu_dxm[m][s]
            << std::endl;
        std::cout << "dwv_dxm[" << m << "][" << s << "] = " << dwv_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 << "d2wu_dxmdxn[" << m << "][" << n << "][" << s << "]["
                << t << "] = " << d2wu_dxmdxn[m][n][s][t] << std::endl;
            std::cout << "d2wv_dxmdxn[" << m << "][" << n << "][" << s << "]["
                << t << "] = " << d2wv_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 area = BaMath::sqrt( cv.a );
    area = area * BaMath::sqrt( area );
#else
    const Float area = 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 = area * ( cv.wuhat.length() - b_u );
    Cv = area * ( cv.wvhat.length() - b_v );

    for ( m = 0; m < 3; ++m ) {
        // $ \pfrac{\Cu}{\xm} = a \pfrac{\whatux}{\xmx} \wu $
        dCu_dxm[ m ] = area * cv.dwhux_dxmx[ m ] * cv.wu;
        // $ \pfrac{\Cv}{\xm} = a \pfrac{\whatvx}{\xmx} \wv $
        dCv_dxm[ m ] = area * cv.dwhvx_dxmx[ m ] * cv.wv;
    }

    // As per [BarWit98] equation just before (11)
    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 ] =
            area * cv.wuhim * cv.dwhux_dxmx[ m ] * cv.dwhux_dxmx[ n ]
            * ( GeMatrix3::IDENTITY - GeMatrix3::outerProduct( cv.wu, cv.wu ) );

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

        d2Cv_dxmdxn[ m ][ n ] =
            area * cv.wvhim * cv.dwhvx_dxmx[ m ] * cv.dwhvx_dxmx[ n ]
            * (GeMatrix3::IDENTITY - GeMatrix3::outerProduct( cv.wv, cv.wv ) );
    }
}

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

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 area = BaMath::sqrt( cv.a );
    area = area * BaMath::sqrt( area );
#else
    Float area = cv.a;
#endif
    // As per [BarWit98] section 4.3
    // $ C = a \wu \cdot \wv $
#if DO_NORM
    // Correction: use normalised wu, wv
    C = area * cv.wu.dot( cv.wv );
#else
    C = area * cv.wuhat.dot( cv.wvhat );
#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 ] = area * (
#if DO_NORM
                cv.dwu_dxm[m][s].dot( cv.wv ) +
                cv.wu.dot( cv.dwv_dxm[m][s] )
#else
                cv.dwhux_dxmx[m] * cv.wvhat[s] +
                cv.wuhat[s] * cv.dwhvx_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 ) = area * (
                cv.d2wu_dxmdxn[m][n][s][t].dot( cv.wv ) +
                cv.dwu_dxm[m][s].dot( cv.dwv_dxm[n][t] ) +
                cv.dwu_dxm[n][t].dot( cv.dwv_dxm[m][s] ) +
                cv.wu.dot( cv.d2wv_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 ) = area * (
                cv.dwhux_dxmx[m] * cv.dwhvx_dxmx[n] +
                cv.dwhux_dxmx[n] * cv.dwhvx_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 nA,nB are equivalent to Baraff & Witkin's n1,n2
    // i.e. unit vectors
    // My nAhat, nBhat are unscaled versions of nA,nB
    // nAhim = 1 / || nAhat ||      "nA hat 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.
    nA *= bv.nAhim / nAhim;
    nAhim = bv.nAhim;
    nB *= bv.nBhim / nBhim;
    nBhim = bv.nBhim;
    e *= bv.ehim / ehim;
    ehim = bv.ehim;
#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 nhatA( (x[2] - x[0]).cross( x[1] - x[0] ) );
    // nAhat inverse magnitude
    nAhim = 1 / nhatA.length();
    // $ \n^A = \frac{\nhat^A}{\norm{\nhat^A}} $
    nA = nAhim * nhatA;

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

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

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

void SimSimulator::BendVars::calcThetas()
{
    // $ \cos \theta = \n^A \cdot \n^B $
    costh = nA.dot( nB );
    // $ \sin \theta = (\n^A \cross \n^B) \cdot \e $
    sinth = nA.cross( nB ).dot( e );
    // 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( nA,nA );
    const GeMatrix3 InBnBt =
        GeMatrix3::IDENTITY - GeMatrix3::outerProduct( nB,nB );
    const GeMatrix3 Ieet =
        GeMatrix3::IDENTITY - GeMatrix3::outerProduct( e, e );

    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} } $
            dnA_dxm[ m ][ s ] = InAnAt * makeSkewRow( qA[ m ], s ) * nAhim;
            // $ \pfrac{\n^B}{\xms} = \left(\I - \n^B(\n^B)^T\right)
            //      \frac{ S_s( \q^B_m ) }{ \norm{\nhat^B} } $
            dnB_dxm[ m ][ s ] = InBnBt * makeSkewRow( qB[ m ], s ) * nBhim;
            // $ \pfrac{\e}{\xms} = (\I - \e\e^T)
            //      \frac{ \Is( q^e_m ) }{ \norm{\ehat} } $
            de_dxm[ m ][ s ] = Ieet * GeVector::axis( s ) * qe[ m ] * ehim;
        }
    }
#else
    for ( UInt32 m = 0; m < 4; ++m ) {
        for ( UInt32 s = 0; s < 3; ++s ) {
            dnA_dxm[ m ][ s ] = makeSkewRow( qA[ m ], s ) * nAhim;
            dnB_dxm[ m ][ s ] = makeSkewRow( qB[ m ], s ) * nAhim;
            de_dxm[ m ][ s ] = GeVector::axis( s ) * qe[ m ] * ehim;
        }
    }
#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( dnA_dxm[ n ][ t ], nA );
            tempm += GeMatrix3::outerProduct( nA, dnA_dxm[ n ][ t ] );
            d2nA_dxmdxn[m][n][s][t] = tempm * SqAm * -nAhim;
            tempm = (GeMatrix3::IDENTITY - GeMatrix3::outerProduct( nA, nA ))
                * (nAhim * nAhim);
            tempv = makeSkewRow( dqAm_dxn[ m ][ n ][ t ], s ) / nAhim;
            tempv -= SqAn.dot( nA ) * 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( dnB_dxm[ n ][ t ], nB );
            tempm += GeMatrix3::outerProduct( nB, dnB_dxm[ n ][ t ] );
            d2nB_dxmdxn[m][n][s][t] = tempm * SqBm * -nBhim;
            tempm = (GeMatrix3::IDENTITY - GeMatrix3::outerProduct( nB, nB ))
                * (nBhim * nBhim);
            tempv = makeSkewRow( dqBm_dxn[ m ][ n ][ t ], s ) / nBhim;
            tempv -= SqBn.dot( nB ) * 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( de_dxm[ n ][ t ], e );
            tempm += GeMatrix3::outerProduct( e, de_dxm[ n ][ t ] );
            d2e_dxmdxn[m][n][s][t] = tempm * GeVector::axis( s ) *
                (-qe[ m ] * ehim);
            tempm = GeMatrix3::IDENTITY - GeMatrix3::outerProduct( e, e );
            tempv = ehim * ehim * qe[ n ] * qe[ m ] * e[ 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 ) * nAhim;
            d2nB_dxmdxn[m][n][s][t] =
                makeSkewRow( dqBm_dxn[m][n][t], s ) * nBhim;
        }
    }
#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] = dnA_dxm[m][s].dot( nB ) + nA.dot( dnB_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 nAxnB( nA.cross( nB ) );
    for( m = 0; m < 4; ++m ) {
        for ( s = 0; s < 3; ++s ) {
            dsinth_dxm[m][s] =
                (
                    dnA_dxm[m][s].cross( nB ) + nA.cross( dnB_dxm[m][s] )
                ).dot( e ) + nAxnB.dot( de_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( nB ) +
                dnA_dxm[m][s].dot( dnB_dxm[n][t] ) +
                dnA_dxm[n][t].dot( dnB_dxm[m][s] ) +
                d2nB_dxmdxn[m][n][s][t].dot( nA );
        }
    }

    // $ \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( nB ) +
                dnA_dxm[m][s].cross( dnB_dxm[n][t] ) +
                dnA_dxm[n][t].cross( dnB_dxm[m][s] ) +
                nA.cross( d2nB_dxmdxn[m][n][s][t] )
            ).dot( e );
            d2sinth_dxmdxn[ m ][ n ]( t, s ) += (
                dnA_dxm[m][s].cross( nB ) +
                nA.cross( dnB_dxm[m][s] )
            ).dot( de_dxm[n][t] );
            d2sinth_dxmdxn[ m ][ n ]( t, s ) += (
                dnA_dxm[n][t].cross( nB ) +
                nA.cross( dnB_dxm[n][t] )
            ).dot( de_dxm[m][s] );
#if DO_NORM
            d2sinth_dxmdxn[ m ][ n ]( t, s ) +=
                nA.cross( nB ).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 << "nA = " << nA << "  nAhim = " << nAhim << std::endl;
    os << "nB = " << nB << "  nBhim = " << nBhim << std::endl;
    os << "e = " << e << "  ehim = " << ehim << 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 << "dnA_dxm[ " << m << " ][ " << s << " ] = "
                << dnA_dxm[ m ][ s ] << std::endl;
            os << "dnB_dxm[ " << m << " ][ " << s << " ] = "
                << dnB_dxm[ m ][ s ] << std::endl;
            os << "de_dxm[ " << m << " ][ " << s << " ] = "
                << de_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-12 ) );
            // 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

---