AnimaL

Tutorial

Documentation

---

Documentation

• Overview
• Modules
• Namespace List
• Class Hierarchy
• Alphabetical List
• Compound List
• File List
• Namespace Members
• Compound Members
• File Members
• Examples

---

DirectManipulation.cpp

Go to the documentation of this file.

#include "DirectManipulation.h"
#include "assertions.h"

#include <map>

namespace animal
{
namespace octree
{

std::vector<Vec3d> computeDirectManipulationResultPseudoInverse( std::vector<FloatingPointType> weights, Vec3d deltaQ )
{

    Require( weights.size() != 0 );

    std::vector<Vec3d> result(weights.size());
    
    
    FloatingPointType sum = 0.0;
    for( unsigned int i=0 ; i<weights.size() ; ++i )
    {
        sum += weights[i]*weights[i];
    }
    
    
    for( unsigned int i=0 ; i<weights.size() ; ++i )
    {
        result[i] = deltaQ * weights[i] / sum;
//      std::cerr << "Weight[" << i << " is " << weights[i] << "and res = " << result[i] << "\n";
    }
    
    return result;  
    /*  
    std::cerr << "DM: deltaQ " << deltaQ << "\n";
    
    unsigned int nControlPoint = weights.size();
    
    // Compute the matrix B with the weights
    HMat B( 3, 3*nControlPoint );
    for( unsigned int i=0 ; i<nControlPoint ; ++i )
    {
        B[0][i] = weights[i];
        B[1][nControlPoint+1 + i] = weights[i];
        B[2][2*nControlPoint+1 + i] = weights[i];
    }
    
    std::cerr << "DM : B is " << B << "\n";
    
    HMat B1( 3*nControlPoint, 3*nControlPoint );
    m_eq_AtB( B1, B, B );
    std::cerr << "DM : B1 is " << B1 << "\n";   
    
    HMat LU(3*nControlPoint,3*nControlPoint);
    m_eq_ludcmp( LU, B1 );
    std::cerr << "DM : LU is " << LU << "\n";
    
    HMat LU_1(3*nControlPoint,3*nControlPoint);
        HVec aux(3*nControlPoint);
        m_eq_luinv( LU_1, LU, aux );
    std::cerr << "DM : LU_1 is " << LU_1 << "\n";
        
    
    HMat B_1( 3*nControlPoint, nControlPoint );
    m_eq_ABt( B_1, LU_1, B );
    std::cerr << "DM : B_1 is " << B_1 << "\n";
    
    // Now we have B^{+} in B_1
    HMat dQ( 3, 1 );
    dQ[0][0] = deltaQ[0]; dQ[1][0] = deltaQ[1]; dQ[2][0] = deltaQ[2];
    
    HMat dP(3*nControlPoint,1);
    m_eq_AB( dP, B_1, dQ );
    
    // Now we have the movements of control points in dP
    // Fill the resulting vectors
    std::vector<Vec3d> result(nControlPoint);
    
    for( unsigned int i=0 ; i<nControlPoint ; ++i )
    {
        result[i][0] = dP[i][0];
        result[i][1] = dP[nControlPoint+1 + i][0];
        result[i][2] = dP[2*nControlPoint+1 + i][0];
        std::cerr << "DM: result[" << i << "] = " << result[i] << "\n";
    }
    
    return result;
    */
    
/*  
    Require( weights.size() != 0 );
    
    std::cerr << "DM: deltaQ " << deltaQ << "\n";
    
    unsigned int nControlPoint = weights.size();
    
    // Compute the matrix B with the weights
    HMat B( 1, nControlPoint );
    for( unsigned int i=0 ; i<nControlPoint ; ++i )
    {
        B[0][i] = weights[i];
        //std::cerr << "DM: weight[" << i << "] = " << weights[i] << "\n";
    }
    
    //std::cerr << "DM : B is " << B << "\n";
    
    HMat B1( nControlPoint, nControlPoint );
    m_eq_AtB( B1, B, B );
    //std::cerr << "DM : B1 is " << B1 << "\n"; 
    
    HMat LU(nControlPoint,nControlPoint);
    m_eq_ludcmp( LU, B1 );
    //std::cerr << "DM : LU is " << LU << "\n";
    
    HMat LU_1(nControlPoint,nControlPoint);
    HVec aux(nControlPoint);
    m_eq_luinv( LU_1, LU, aux );
    //std::cerr << "DM : LU_1 is " << LU_1 << "\n";
        
    
    HMat B_1( nControlPoint, 1 );
    m_eq_ABt( B_1, LU_1, B );
    //std::cerr << "DM : B_1 is " << B_1 << "\n";
    
    // Now we have B^{+} in B_1
    HMat dQ( 1, 3 );
    dQ[0][0] = deltaQ[0]; dQ[0][1] = deltaQ[1]; dQ[0][2] = deltaQ[2];
    
    HMat dP(nControlPoint,3);
    m_eq_AB( dP, B_1, dQ );
    
    // Now we have the movements of control points in dP
    // Fill the resulting vectors
    std::vector<Vec3d> result(nControlPoint);
    
    for( unsigned int i=0 ; i<nControlPoint ; ++i )
    {
        result[i][0] = dP[i][0];
        result[i][1] = dP[i][1];
        result[i][2] = dP[i][2];
        //std::cerr << "DM: result[" << i << "] = " << result[i] << "\n";
    }
    
    return result;
*/
}


typedef struct {
    ConstrainedVertex *_vert;
    FloatingPointType _w;
} VertexWeight ;

void computeDirectManipulationSkinning(
            Vec3d deltaQ,
            std::vector<FloatingPointType> weights,
            std::vector<Vec3d> relPos,          // the alpha/beta/gamma params
            std::vector<ConstrainedVertex*> framesVertices, // in data
            std::vector<ConstrainedVertex*> & verticesToMove,   // the vertices that must be moved
            std::vector<Vec3d> & deltaP             // associated delta
            )
{
//  std::cerr << "computeDirectManipulationSkinning\n";
    const unsigned int nVert = weights.size();
    
    Require( relPos.size() == nVert );
    Require( framesVertices.size() == nVert );
    Require( nVert != 0 );
    
    verticesToMove.clear();
    deltaP.clear();

    FloatingPointType weightSum = 0.0;
    for( std::vector<FloatingPointType>::iterator it=weights.begin() ; it != weights.end() ; ++it )
    {
        weightSum += *it;
    }
    
    //
    // Create a table containing the unique indices for each constrained vertex
    //
    std::map<ConstrainedVertex*,unsigned int> indexMap;
    //
    // Create simulteanously a map for each vertex
    // of the FREE connected vertices (defining the frame's vectors)
    // Have a joint definition of their associated weight
    //
    std::map<ConstrainedVertex*,FloatingPointType> jacobianWeights;
    
    {
        unsigned int lastIndex = 0;
        for( unsigned int i=0 ; i<nVert ; ++i )
        {
//          std::cerr << "Treating vertex " << i << "/" << nVert << "\n";
            
            ConstrainedVertex *cv = framesVertices[i];
            
            // Add it itself and add the vertices for the parents
//          indexMap[ cv ] = lastIndex++;
            // From \sum w_i Q_i = \sum w_i ( P_i + B_i )
            jacobianWeights[ cv ] += weights[i];
            
//          std::cerr << "Adding1 " << weights[i] << " to cv " << *cv << " (" << jacobianWeights[ cv ] << ")\n";
            
//          std::cerr << "\tPointer is " << cv << " and weight is " << jacobianWeights[ cv ] << " (have added " << weights[i] << ")\n";
            // From B_i = \alpha U_i + \beta V_i + \gamma W_i
            // where U_i = k_{u,0,i} U_{P_{0,i}} + k_{u,1,i} U_{P_{1,i}}  - ( k_{u,0,i} + k_{u,1,i} ) P_i 
            for( unsigned int dir=0 ; dir<3 ; ++dir )
            {
                FloatingPointType k = -(cv->_connectedVerticesFactors[dir][0]+cv->_connectedVerticesFactors[dir][1]);
                jacobianWeights[cv] += weights[i] * relPos[i][dir] * k;
                
//              std::cerr << "Adding2 " << weights[i] * relPos[i][dir] * k << " to cv " << *cv << " (" << jacobianWeights[ cv ] << ")\n";
                
                //
                // Now find our free parents and add their weight
                // First for the 0's component
                //
                for( unsigned int comp=0 ; comp<=1 ; comp++ )
                {
                    if( cv->_connectedVerticesFactors[dir][comp] != 0.0 )
                    {
                        std::list<VertexWeight> parentsWeights;
                    
                        VertexWeight tmp;
                        tmp._vert = cv->_connectedVertices[dir][comp];
                        tmp._w = cv->_connectedVerticesFactors[dir][comp] * relPos[i][dir] * weights[i];
                        
                        parentsWeights.push_back( tmp );
                        while( !parentsWeights.empty() )
                        {
                            tmp = parentsWeights.front();
                            parentsWeights.pop_front();
                            if( tmp._vert->isFree() )
                            {
                                jacobianWeights[tmp._vert] += tmp._w;
//                              std::cerr << "Adding3 " << tmp._w << " to cv " << *tmp._vert << " (" << jacobianWeights[ tmp._vert ] << ")\n";
                            }
                            else
                            {
                                unsigned int nP = tmp._vert->getGeoLink()->getNGeoParents();
                                FloatingPointType weightP = tmp._w / (FloatingPointType)nP;
                                for( unsigned int j=0 ; j<nP ; ++j )
                                {
                                    VertexWeight tmp2;
                                    tmp2._vert = tmp._vert->getGeoLink()->getGeoParent( tmp._vert->getGeoLink()->getGeoParentId(j) );
                                    tmp2._w = weightP;
                                    parentsWeights.push_back( tmp2 );
                                }
                            }
                        }
                    }
                }
                
            }
        }
    }
    
    
//  Vec3d Q;
    std::vector<FloatingPointType> freeWeights( jacobianWeights.size() );
    verticesToMove.resize( freeWeights.size() );
    
    double tmp = 0.0;
    unsigned int i=0;
    for( std::map<ConstrainedVertex*,FloatingPointType>::iterator it = jacobianWeights.begin() ;
        it != jacobianWeights.end() ;
        ++it, ++i )
    {
        (*it).second /= weightSum;
        freeWeights[i] = (*it).second;
        verticesToMove[i] = (*it).first;
        
        tmp += freeWeights[i];
    }
//  std::cerr << "weight sum is " << tmp << " and original was : " << weightSum << "\n";
    
    //
    // now call the solver
    //
    deltaP = computeDirectManipulationResultPseudoInverse( freeWeights, deltaQ );
    
//  Vec3d tmpQ;
//  for( i=0 ; i<deltaP.size() ; ++i )
//  {
//      tmpQ += freeWeights[i]*deltaP[i];
//  }
//  std::cerr << "\ttmpQ is " << tmpQ << "\n";
//  
//  Vec3d oldQ;
//  Vec3d newQ;
//  for( i=0 ; i<deltaP.size() ; ++i )
//  {
//      oldQ += freeWeights[i] * verticesToMove[i]->getPosition();
//      newQ += freeWeights[i] * (verticesToMove[i]->getPosition() + deltaP[i]);
//  }
//  std::cerr << "\tOldQ is " << oldQ << "\n";
//  std::cerr << "\tNEWQ is " << newQ << "\n";
    
//  std::cerr << "Res is " << Q << "\n";
//  std::vector<Vec3d> computeDirectManipulationResultPseudoInverse( std::vector<FloatingPointType> weights, Vec3d deltaQ )
    
//  Vec3d verif;
//  for( unsigned int i=0 ; i<nVert ; ++i )
//  {
//      Vec3d pos = framesVertices[i]->getPosition();
//      
//      std::cerr << "Frame " << i << " : " << weights[i] << " relPos : " << relPos[i] << "\n";
//      
//      Vec3d vecs[3];
//      for( unsigned int dir=0 ; dir<3 ; ++dir )
//      {
//          vecs[dir] = (framesVertices[i]->_connectedVertices[dir][0]->getPosition() - framesVertices[i]->getPosition()) * framesVertices[i]->_connectedVerticesFactors[dir][0];
//          if( framesVertices[i]->_connectedVerticesFactors[dir][1] != 0 )
//          {
//              vecs[dir] += (framesVertices[i]->_connectedVertices[dir][1]->getPosition() - framesVertices[i]->getPosition()) * framesVertices[i]->_connectedVerticesFactors[dir][1];
//          }
//          pos += vecs[dir] * relPos[i][dir];
//      }
//      std::cerr << "position is : " << pos << "\n";
//      
//      verif += pos * weights[i];
//  }
//  std::cerr << "Final : " << verif << "\n";
}

void computeDirectManipulationSkinningFrames(
                    Vec3d deltaQ,
                    std::vector<FloatingPointType> weights,
                    std::vector<Vec3d> relPos,
                    std::vector<ConstrainedVertex*> framesVertices, // in data
                    std::vector< std::vector<Vec3d> > & deltaFrames  // results here
                    )
{
    deltaFrames.clear();
    deltaFrames.resize( framesVertices.size() );
    
    std::vector<FloatingPointType> framesWeights;
    for( unsigned int i=0 ; i<framesVertices.size() ; ++i )
    {
//      RHS -= weights[i] * framesVertices[i]->getPosition();
        framesWeights.push_back( weights[i] * relPos[i][0] );
        framesWeights.push_back( weights[i] * relPos[i][1] );
        framesWeights.push_back( weights[i] * relPos[i][2] );
    }
    
    std::vector<Vec3d> deltaV;
    deltaV = computeDirectManipulationResultPseudoInverse( framesWeights, deltaQ );
    
    for( unsigned int i=0 ; i<framesVertices.size() ; ++i )
    {
        deltaFrames[i].push_back( deltaV[i*3+0] );
        deltaFrames[i].push_back( deltaV[i*3+1] );
        deltaFrames[i].push_back( deltaV[i*3+2] );
    }
}

void computeDirectManipulationSkinningVerticesAndFrames(
                    Vec3d deltaQ,
                    std::vector<FloatingPointType> weights,
                    std::vector<Vec3d> relPos,
                    std::vector<ConstrainedVertex*> framesVertices, // in data
                    std::vector< Vec3d > & deltas  // results here
                    )
{
    std::vector<FloatingPointType> myWeights;
    for( unsigned int i=0 ; i<weights.size() ; ++i )
    {
        myWeights.push_back( weights[i] );
        myWeights.push_back( weights[i] * relPos[i][0] );
        myWeights.push_back( weights[i] * relPos[i][1] );
        myWeights.push_back( weights[i] * relPos[i][2] );
    }
    
    deltas = computeDirectManipulationResultPseudoInverse( myWeights, deltaQ );
}

void computeDirectManipulationSkinningVertices(
                    Vec3d deltaQ,
                    std::vector<FloatingPointType> weights,
                    std::vector<Vec3d> relPos,
                    std::vector<ConstrainedVertex*> framesVertices, // in data
                    std::vector< Vec3d > & deltas  // results here
                    )
{
    std::vector<FloatingPointType> myWeights;
    for( unsigned int i=0 ; i<weights.size() ; ++i )
    {
        myWeights.push_back( weights[i] );
    }
    
    deltas = computeDirectManipulationResultPseudoInverse( myWeights, deltaQ );
}

}
}

---