//------------------------------------------------------------------------------ //! inline void visitSplitNode( const BIH::Node& node, const Vec2f& range, Vector<uint>& stack ) { DBG_BLOCK( os_bih, "visitSplitNode(" << node << ", " << range << ", " << stack.size() << " elems in stack" << ")" ); if( range(0) <= node._plane[0] ) { if( node._plane[1] <= range(1) ) { DBG_MSG( os_bih, "Left and right test pass, pushing " << node.index() << " and next" ); // Visit left node, then right stack.pushBack( node.index() + 1 ); stack.pushBack( node.index() ); } else { DBG_MSG( os_bih, "Left test passes, pushing " << node.index() ); // Visit left node only stack.pushBack( node.index() ); } } else { if( node._plane[1] <= range(1) ) { DBG_MSG( os_bih, "Right test passes, pushing " << node.index() + 1 ); // Visit right node only stack.pushBack( node.index() + 1 ); } } }
//------------------------------------------------------------------------------ //! void CoreXGL::performShow() { DBG_BLOCK( os_xgl, "CoreXGL::performShow" ); // Show... todo }
//------------------------------------------------------------------------------ //! Append anim2 at the end of anim1 (no blending whatsoever). //! If rates differ, the rate of anim1 is used for the final animation. RCP<SkeletalAnimation> Puppeteer::concatenate( SkeletalAnimation* anim1, SkeletalAnimation* anim2 ) { DBG_BLOCK( os_pup, "Puppeteer::concatenate(" << anim1 << ", " << anim2 << ")" ); anim1->makeRelative(); anim2->makeRelative(); RCP<SkeletalAnimation> anim = anim1->clone(); if( anim2->numPoses() > 0 ) { RCP<SkeletalAnimation> animToAppend; if( anim1->rate() == anim2->rate() ) { animToAppend = anim2; } else { animToAppend = resample( anim2, anim1->rate() ); } // Remove the first animation's last frame (a copy of frame 0). uint np1 = anim->numPoses() - 1; anim->removePose( np1 ); // Add the second animation's poses. uint np2 = animToAppend->numPoses(); anim->reservePoses( np1 + np2 ); for( uint p = 0; p < np2; ++p ) { anim->addPose( animToAppend->pose(p)->clone().ptr() ); } } return anim; }
//------------------------------------------------------------------------------ //! Create a transition frame from one animation (startAnim) to another one //! (endAnim) using the specified factor. void Puppeteer::transitionFrame( SkeletalAnimation* startAnim, SkeletalAnimation* endAnim, float factor, SkeletalPose& dstPose ) { DBG_BLOCK( os_pup, "Puppeteer::transitionFrame(" << startAnim << ", " << endAnim << ", " << factor << ")" ); startAnim->makeRelative(); endAnim->makeRelative(); // Compute poses. SkeletalPose* p0 = startAnim->pose( startAnim->numPoses()-1 ); SkeletalPose* p1 = endAnim->pose(0); const SkeletalPose::BoneContainer& b0 = p0->bones(); const SkeletalPose::BoneContainer& b1 = p1->bones(); SkeletalPose::BoneContainer& db = dstPose.bones(); //Reff ref = p0->referential().slerp( p1->referential(), factor ); Reff ref = p0->referential().nlerp( p1->referential(), factor ); dstPose.referential( ref ); CHECK( b0.size() == b1.size() ); CHECK( b0.size() == db.size() ); const uint numBones = (uint)db.size(); for( uint i = 0; i < numBones; ++i ) { //db[i] = b0[i].slerp( b1[i], factor ); db[i] = b0[i].nlerp( b1[i], factor ); } }
//------------------------------------------------------------------------------ //! Retrieves an interpolated referential at time 't' in the animation. Reff AnimationRail::get( const float t ) { DBG_BLOCK( os_ar, "AnimationRail::get(" << t << ")" ); Reff ref; uint idx = findIndex( t, _lastIndexUsed ); DBG_MSG( os_ar, "idx=" << idx << "/" << numAnchors() ); if( idx >= numAnchors() ) { // Last anchor. ref = _anchors.back()._ref; DBG_MSG( os_ar, "Last anchor, returning: " << ref ); } else { const Anchor& a0 = _anchors[idx]; const Anchor& a1 = _anchors[idx+1]; float delta = a1._time - a0._time; float cur = t - a0._time; float f = interpolate( cur / delta ); ref = a0._ref.slerp( a1._ref, f ); DBG_MSG( os_ar, "Interpolating " << "#" << idx << "(" << a0._time << ", " << a0._ref << ")" ); DBG_MSG( os_ar, " with " << "#" << idx+1 << "(" << a1._time << ", " << a1._ref << ")" ); DBG_MSG( os_ar, " delta=" << delta << " cur=" << cur << " f=" << f << " --> " << ref ); } _lastIndexUsed = idx; return ref; }
//------------------------------------------------------------------------------ //! Blends between 2 animations at a specific frame. //! This is done by interpolating 2 frames of animA, then 2 frames of animB, //! then blending the 2 resulting frames together according to the factor. //! @param animA: The first animation. //! @param timeA: A time hint for the first animation. //! @param animB: The second animation. //! @param timeB: A time hint for the second animation. //! @param factor: A blend factor to merge animA with animB. //! @param dstPose: A pre-allocated pose into which to store the result. void Puppeteer::blendFrame( SkeletalAnimation* animA, float timeA, SkeletalAnimation* animB, float timeB, float factor, SkeletalPose& dstPose ) { DBG_BLOCK( os_pup, "Puppeteer::blendFrame(" << animA << ", " << timeA << ", " << animB << ", " << timeB << ", " << factor << ")" ); animA->makeRelative(); animB->makeRelative(); // Compute poses. SkeletalPose* pA0; SkeletalPose* pA1; float tA; SkeletalPose* pB0; SkeletalPose* pB1; float tB; animA->getPoses( timeA, pA0, pA1, tA ); animB->getPoses( timeB, pB0, pB1, tB ); DBG_MSG( os_pup, "A: " << timeA << " --> " << tA ); DBG_MSG( os_pup, "B: " << timeB << " --> " << tB ); const SkeletalPose::BoneContainer& bA0 = pA0->bones(); const SkeletalPose::BoneContainer& bA1 = pA1->bones(); const SkeletalPose::BoneContainer& bB0 = pB0->bones(); const SkeletalPose::BoneContainer& bB1 = pB1->bones(); //Reff refA = pA0->referential().slerp( pA1->referential(), tA ); //Reff refB = pB0->referential().slerp( pB1->referential(), tB ); //Reff ref = refA.slerp( refB, factor ); Reff refA = pA0->referential().nlerp( pA1->referential(), tA ); Reff refB = pB0->referential().nlerp( pB1->referential(), tB ); Reff ref = refA.nlerp( refB, factor ); dstPose.referential( ref ); SkeletalPose::BoneContainer& dstBones = dstPose.bones(); CHECK( bA0.size() == bB0.size() ); CHECK( bA0.size() == dstBones.size() ); const uint nBones = uint(dstBones.size()); for( uint i = 0; i < nBones; ++i ) { //Quatf sorient = bA0[i].slerp( bA1[i], tA ); //Quatf eorient = bB0[i].slerp( bB1[i], tB ); //Quatf orient = sorient.slerp( eorient, factor ); Quatf sorient = bA0[i].nlerp( bA1[i], tA ); Quatf eorient = bB0[i].nlerp( bB1[i], tB ); Quatf orient = sorient.nlerp( eorient, factor ); dstBones[i] = orient; } }
//------------------------------------------------------------------------------ //! Retrieve a portion of anim between the specified start and end positions. RCP<SkeletalAnimation> Puppeteer::cut( SkeletalAnimation* srcAnim, uint startPose, uint endPose ) { DBG_BLOCK( os_pup, "Puppeteer::cut(" << srcAnim << ", " << startPose << ", " << endPose << ")" ); RCP<SkeletalAnimation> dstAnim = srcAnim->clone( startPose, endPose ); return dstAnim; }
//------------------------------------------------------------------------------ //! void CoreXGL::resize ( int w, int h ) { DBG_BLOCK( os_xgl, "CoreXGL::resize" ); _size.x = w; _size.y = h; performResize( w, h ); }
//------------------------------------------------------------------------------ //! bool NullManager::setData( const RCP<Texture>& /*texture*/, const uint /*level*/, const void* /*data*/, const bool /*skipDefinedRegionUpdate*/ ) { DBG_BLOCK( os_nm, "NullManager::setData( tex )" ); return true; }
//------------------------------------------------------------------------------ //! CoreXGL::CoreXGL() : _dblClickDelay( 300 ) { DBG_BLOCK( os_xgl, "CoreXGL::CoreXGL" ); addRoot( "" ); _mainPointerID = Core::createPointer().id(); // Create window. initWin(); }
//------------------------------------------------------------------------------ //! bool AnimationRail::performGet( VMState* vm ) { DBG_BLOCK( os_ar, "AnimationRail::performGet" ); const char* str = VM::toCString( vm, -1 ); switch( _attributes[str] ) { case ATTRIB_ADD_ANCHOR: { VM::push( vm, this, addAnchorVM ); } return true; case ATTRIB_ADD_ANCHORS: { VM::push( vm, this, addAnchorsVM ); } return true; case ATTRIB_GET: { VM::push( vm, this, getVM ); } return true; case ATTRIB_GET_ANCHOR: { VM::push( vm, this, getAnchorVM ); } return true; case ATTRIB_GET_ANCHORS: { VM::push( vm, this, getAnchorsVM ); } return true; case ATTRIB_NORMALIZE: { VM::push( vm, this, normalizeVM ); } return true; case ATTRIB_NUM_ANCHORS: { VM::push( vm, numAnchors() ); } return true; case ATTRIB_REMOVE_ALL_ANCHORS: { VM::push( vm, this, removeAllAnchorsVM ); } return true; case ATTRIB_REMOVE_ANCHOR: { VM::push( vm, this, removeAnchorVM ); } return true; case ATTRIB_SET_ANCHOR: { VM::push( vm, this, setAnchorVM ); } return true; default: { } break; } return false; }
//------------------------------------------------------------------------------ //! void CoreXGL::performExec() { DBG_BLOCK( os_xgl, "CoreXGL::performExec" ); // Create Gfx manager and context. RCP<Gfx::GLContext_GLX> cntx = new Gfx::GLContext_GLX( _display, _window ); Core::gfx( Gfx::Manager::create( cntx.ptr() ) ); initializeGUI(); readyToExec(); performShow(); bool done = false; XEvent event; _timer.restart(); #if PROFILE_EVENTS EventProfiler& profiler = Core::profiler(); profiler.add( EventProfiler::LOOPS_BEGIN ); #endif while( !done ) { #if PROFILE_EVENTS profiler.add( EventProfiler::LOOP_BEGIN ); #endif while( XPending( _display ) > 0 ) { XNextEvent( _display, &event ); done = handleEvents( event ); } processEvents(); executeAnimators( _timer.elapsed() ); render(); #if PROFILE_EVENTS profiler.add( EventProfiler::LOOP_END ); #endif } #if PROFILE_EVENTS profiler.add( EventProfiler::LOOPS_BEGIN ); #endif performHide(); finalizeGUI(); }
//------------------------------------------------------------------------------ //! inline void visitLeafNode( const BIH::Node& node, const Vector<uint>& ids, Vector<uint>& dst ) { DBG_BLOCK( os_bih, "visitLeafNode(" << node << ")" ); uint index = node.index(); DBG_MSG( os_bih, "Adding [" << node.index() << ", " << node.index() + node._numElements - 1 << "]" ); for( uint i = 0; i < node._numElements; ++i ) { dst.pushBack( ids[index + i] ); } }
void recursion(int x) { DBG_BLOCK("%d levels to go",x); usleep((random()%300)*1000); if(x>0) { x--; DBG(DUMP,"about go invoke recursion with %d",x); DBG(INFO,"recursion continues"); recursion(x); } else DBG(INFO,"recursion reached end"); }
//------------------------------------------------------------------------------ //! inline void visitClipNode( const BIH::Node& node, const Vec2f& range, Vector<uint>& stack ) { DBG_BLOCK( os_bih, "visitSplitNode(" << node << ", " << range << ", " << stack.size() << " elems in stack" << ")" ); if( node._plane[0] <= range(1) && range(0) <= node._plane[1] ) { DBG_MSG( os_bih, "Test passes" ); stack.pushBack( node.index() ); } }
//------------------------------------------------------------------------------ //! RCP<SkeletalAnimation> Puppeteer::cycle( SkeletalAnimation* animation, float fraction ) { DBG_BLOCK( os_pup, "Puppeteer::cycle(" << animation << ", " << fraction << ")" ); RCP<SkeletalAnimation> anim = animation->clone(); anim->makeRelative(); fraction = CGM::clamp( fraction, 0.0f, 1.0f ); uint offset = uint((anim->numPoses()-1)*fraction); uint numPoses = anim->numPoses()-offset; // Compute delta transformations. SkeletalPose* sp = anim->pose(0); SkeletalPose* ep = anim->pose( anim->numPoses()-1 ); Reff dref = sp->referential() * ep->referential().getInversed(); SkeletalPose::BoneContainer& b0 = sp->bones(); SkeletalPose::BoneContainer& b1 = ep->bones(); Vector<Quatf> drot( b0.size() ); for( uint i = 0; i < b0.size(); ++i ) { drot[i] = b0[i] * b1[i].getInversed(); } // Compute poses. for( uint p = 0; p < numPoses; ++p ) { SkeletalPose* p0 = anim->pose( offset+p ); SkeletalPose::BoneContainer& b0 = p0->bones(); float t = float(p) / float(numPoses-1); //Reff ref = p0->referential().slerp( dref*p0->referential(), t ); Reff ref = p0->referential().nlerp( dref*p0->referential(), t ); p0->referential( ref ); for( uint i = 0; i < b0.size(); ++i ) { //b0[i] = b0[i].slerp( drot[i]*b0[i], t ); b0[i] = b0[i].nlerp( drot[i]*b0[i], t ); } } anim->cyclic( true ); // We are now cyclic. return anim; }
//------------------------------------------------------------------------------ //! bool BIH::findElementsInside( const AABBoxf& box, Vector<uint>& dst, InsideFunc inside, void* data ) const { DBG_BLOCK( os_bih, "BIH::findElementsInside(" << box << ")" ); DBG( print( os_bih.stream(), ">> " ) ); if( _nodes.empty() ) { return false; } Vector<uint> stack; stack.pushBack( 0 ); while( !stack.empty() ) { DBG_MSG( os_bih, "Node #" << stack.back() ); const Node& node = _nodes[stack.back()]; stack.popBack(); switch( node.flags() ) { case Node::BIH_NODE_SPLIT_X: visitSplitNode( node, box.slabX(), stack ); break; case Node::BIH_NODE_SPLIT_Y: visitSplitNode( node, box.slabY(), stack ); break; case Node::BIH_NODE_SPLIT_Z: visitSplitNode( node, box.slabZ(), stack ); break; case Node::BIH_NODE_LEAF: visitLeafNode( node, _ids, dst, box, inside, data ); break; case Node::BIH_NODE_CLIP_X: visitClipNode( node, box.slabX(), stack ); break; case Node::BIH_NODE_CLIP_Y: visitClipNode( node, box.slabY(), stack ); break; case Node::BIH_NODE_CLIP_Z: visitClipNode( node, box.slabZ(), stack ); break; } } return !dst.empty(); }
//------------------------------------------------------------------------------ //! Reverses the animation. RCP<SkeletalAnimation> Puppeteer::reverse( SkeletalAnimation* srcAnim ) { DBG_BLOCK( os_pup, "Puppeteer::reverse(" << srcAnim << ")" ); const uint numPoses = srcAnim->numPoses(); RCP<SkeletalAnimation> dstAnim = new SkeletalAnimation(); dstAnim->skeleton( srcAnim->skeleton() ); dstAnim->rate( srcAnim->rate() ); dstAnim->reservePoses( numPoses ); for( uint p = 0; p < numPoses; ++p ) { SkeletalPose* srcPose = srcAnim->pose( numPoses - 1 - p ); dstAnim->addPose( srcPose->clone().ptr() ); } return dstAnim; }
//------------------------------------------------------------------------------ //! bool NullManager::setData( const RCP<Texture>& /*texture*/, const uint /*level*/, const uint /*slice*/, const uint /*offset_x*/, const uint /*offset_y*/, const uint /*width*/, const uint /*height*/, const void* /*data*/, const bool /*skipDefinedRegionUpdate*/ ) { DBG_BLOCK( os_nm, "NullManager::setData( 2D tex )" ); return true; }
//------------------------------------------------------------------------------ //! RCP<Texture> NullManager::createCubeTexture( const uint edgeLength, const TextureFormat tfmt, const TextureChannels chOrder, const TextureFlags flags ) { DBG_BLOCK( os_nm, "NullManager::createCubeTexture()" ); RCP<Texture> texture = new Texture(); texture->setCubemap(edgeLength); texture->format(tfmt); texture->channelOrder(chOrder); texture->flags(flags); return texture; }
//------------------------------------------------------------------------------ //! RCP<SkeletalAnimation> Puppeteer::transition( SkeletalAnimation* startAnim, SkeletalAnimation* endAnim, float duration ) { DBG_BLOCK( os_pup, "Puppeteer::transition(" << startAnim << ", " << endAnim << ", " << duration << ")" ); RCP<SkeletalAnimation> anim = new SkeletalAnimation(); anim->skeleton( startAnim->skeleton() ); startAnim->makeRelative(); endAnim->makeRelative(); float rate = CGM::max( startAnim->rate(), endAnim->rate() ); uint numPoses = uint(rate*duration) + 1; anim->reservePoses( numPoses ); anim->rate( rate ); anim->velocity( Vec3f(0.0f) ); anim->offset( (startAnim->offset() + endAnim->offset())*0.5f ); // Compute poses. SkeletalPose* p0 = startAnim->pose( startAnim->numPoses()-1 ); SkeletalPose* p1 = endAnim->pose(0); const SkeletalPose::BoneContainer& b0 = p0->bones(); const SkeletalPose::BoneContainer& b1 = p1->bones(); for( uint p = 0; p < numPoses; ++p ) { float t = float(p) / float(numPoses-1); //Reff ref = p0->referential().slerp( p1->referential(), t ); Reff ref = p0->referential().nlerp( p1->referential(), t ); SkeletalPose* pose = anim->addPose( ref ); pose->reserveBones( uint(b0.size()) ); for( uint i = 0; i < b0.size(); ++i ) { //pose->addBone( b0[i].slerp( b1[i], t ) ); pose->addBone( b0[i].nlerp( b1[i], t ) ); } } return anim; }
//------------------------------------------------------------------------------ //! RCP<Texture> NullManager::create2DTexture( const uint width, const uint height, const TextureFormat tfmt, const TextureChannels chOrder, const TextureFlags flags ) { DBG_BLOCK( os_nm, "NullManager::create3DTexture()" ); RCP<Texture> texture = new Texture(); texture->set2D(width, height); texture->format(tfmt); texture->channelOrder(chOrder); texture->flags(flags); return texture; }
//------------------------------------------------------------------------------ //! MOVE TO ALL LAYERS RCP<ConstantBuffer> NullManager::createConstants( const RCP<Program>& program, const Vector< Set<String> >* ignoreGroups, uint32_t* usedIgnoreGroup ) { DBG_BLOCK( os_nm, "NullManager::createConstants()" ); ConstantBuffer::Container constants; size_t size = getConstants( program, constants, ignoreGroups, usedIgnoreGroup ); if( !constants.empty() ) { return RCP<ConstantBuffer>( new ConstantBuffer( constants, size ) ); } else { return RCP<ConstantBuffer>( new ConstantBuffer(0) ); } }
//------------------------------------------------------------------------------ //! Moves the animation into the specified referential. RCP<SkeletalAnimation> Puppeteer::applyReferential( SkeletalAnimation* animation, const Reff& ref ) { DBG_BLOCK( os_pup, "Puppeteer::applyReferential(" << animation << ", " << ref << ")" ); RCP<SkeletalAnimation> anim = animation->clone(); //anim->makeRelative(); anim->makeAbsolute(); uint numPoses = anim->numPoses(); for( uint p = 0; p < numPoses; ++p ) { SkeletalPose* pose = anim->pose( p ); pose->referential( ref * pose->referential() ); } //anim->makeRelative(); //anim->velocity( anim->velocity() ) return anim; }
//------------------------------------------------------------------------------ //! void AnimationRail::init( VMState* vm ) { DBG_BLOCK( os_ar, "AnimationRail::init" ); if( VM::isTable( vm, -1 ) ) { // Start iterating at index 0 (nil, pushed below). VM::push( vm ); while( VM::next( vm, -2 ) ) { // Let the performSet() routine handle the various assignments. performSet( vm ); // Pop the value, but keep the key. VM::pop( vm, 1 ); } // Creation-only parameters... } }
//------------------------------------------------------------------------------ //! void AnchoredPosSpring::prePositionStep( double step ) { DBG_BLOCK( os_spr, "AnchoredPosSpring::prePositionStep(" << step << ")" ); // Compute world anchors. Vec3f worldAnchorA = _bodyA->transform() * _anchorA; Vec3f x = (_anchorB - worldAnchorA); float dist = x.length(); if( dist > CGConstf::epsilon() ) { x *= (1.0f/dist); } else { x = Vec3f( 0.0f, -1.0f, 0.0f ); } x *= (dist - _restLength); // Apply friction using the velocity from B to A. Vec3f deltaV = -_bodyA->velocity( worldAnchorA ); // Compute impulse to apply. // F = -kx - dv = k*(-x) + d*(-v) // p = F * t Vec3f p = x * (_stiffness * (float)step) + deltaV * (_damping * (float)step); _bodyA->applyImpulse( p, worldAnchorA ); DBG_MSG( os_spr, "WorldAnchorA: " << worldAnchorA ); DBG_MSG( os_spr, "AnchorB: " << _anchorB ); DBG_MSG( os_spr, "A->B: " << _anchorB - worldAnchorA ); DBG_MSG( os_spr, "dist: " << dist ); DBG_MSG( os_spr, "x: " << x ); DBG_MSG( os_spr, "deltaV: " << deltaV ); DBG_MSG( os_spr, "p: " << p ); }
//------------------------------------------------------------------------------ //! inline void visitLeafNode( const BIH::Node& node, const Vector<uint>& ids, Vector<uint>& dst, const AABBoxf& box, BIH::InsideFunc inside, void* data ) { DBG_BLOCK( os_bih, "visitLeafNode(" << node << ")" ); uint index = node.index(); DBG_MSG( os_bih, "Adding [" << node.index() << ", " << node.index() + node._numElements - 1 << "]" ); for( uint i = 0; i < node._numElements; ++i ) { const uint id = index + i; if( inside(box, ids[id], data) ) { dst.pushBack( ids[id] ); } } }
//------------------------------------------------------------------------------ //! Removes the specified axis from the rotation of the root. RCP<SkeletalAnimation> Puppeteer::dropRotation( SkeletalAnimation* animation, const Vec3f& axis ) { DBG_BLOCK( os_pup, "Puppeteer::dropRotation(" << animation << ", " << axis << ")" ); RCP<SkeletalAnimation> anim = animation->clone(); Vec3f pAxis = Vec3f::perpendicular( axis ); const uint numPoses = anim->numPoses(); StdOut << pAxis << nl; for( uint p = 0; p < numPoses; ++p ) { SkeletalPose* pose = anim->pose( p ); const Quatf& q = pose->orientation(); Vec3f src = q * pAxis; Vec3f dst = src - src.projectOnto( axis ); Quatf r = Quatf::twoVecs( src, dst ); pose->orientation( r ); StdOut << p << ": " << q << "," << src << "," << dst << "," << r << nl; //pose->orientation( Quatf::identity() ); } return anim; }
//------------------------------------------------------------------------------ //! bool AnimationRail::performSet( VMState* vm ) { DBG_BLOCK( os_ar, "AnimationRail::performSet" ); const char* str = VM::toCString( vm, -2 ); switch( _attributes[str] ) { case ATTRIB_ADD_ANCHOR: case ATTRIB_ADD_ANCHORS: case ATTRIB_GET: case ATTRIB_GET_ANCHOR: case ATTRIB_GET_ANCHORS: case ATTRIB_NORMALIZE: case ATTRIB_NUM_ANCHORS: case ATTRIB_REMOVE_ALL_ANCHORS: case ATTRIB_REMOVE_ANCHOR: case ATTRIB_SET_ANCHOR: //Read-only return true; default: break; } return false; }
//------------------------------------------------------------------------------ //! void AnchoredSpring::preStep( double step ) { DBG_BLOCK( os_spr, "AnchoredSpring::prePositionStep(" << step << ")" ); // Compute world anchors. Vec3f worldAnchorA = _bodyA->transform() * _anchorA; Vec3f x = (_anchorB.position() - worldAnchorA); float dist = x.length(); if( dist > CGConstf::epsilon() ) { x *= (1.0f/dist); } else { x = Vec3f( 0.0f, -1.0f, 0.0f ); } x *= (dist - _restLength); // Apply friction using the velocity from B to A. Vec3f deltaV = -_bodyA->velocity( worldAnchorA ); // Compute impulse to apply. // F = -kx - dv = k*(-x) + d*(-v) // p = F * t Vec3f p = x * (_stiffness * (float)step) + deltaV * (_damping * (float)step); _bodyA->applyImpulse( p, worldAnchorA ); /** StdErr << "anchorA=" << _anchorA << " worldAnchorA=" << worldAnchorA << " deltaV=" << deltaV << " bodyA=" << _bodyA->referential() << " vel=" << _bodyA->linearVelocity() << " p=" << p << nl; **/ // Compute the transformation needed to end a the desired orientation. Quatf curO = _bodyA->referential().orientation(); Quatf desO = _anchorB.orientation(); Quatf trfO = desO * curO.getInversed(); // Compute the angular velocity related to this transformation. Vec3f axis; float angle; trfO.toAxisAngle( axis, angle ); Vec3f angularVelocity = axis*(angle/(float)step); Vec3f diffAV = angularVelocity - _bodyA->angularVelocity(); // Clamp angular velocity. float maxAV = _maxTorque*(float)step/_bodyA->mass(); float diffAVLen = diffAV.length(); if( diffAVLen > maxAV ) { diffAV *= maxAV/diffAVLen; } // Apply angular velocity. _bodyA->angularVelocity( _bodyA->angularVelocity() + diffAV ); /** StdErr << "curO=" << curO << " desO=" << desO << " angVel=" << angularVelocity << " body=" << _bodyA->angularVelocity() << " diffAV=" << diffAV << nl; **/ // FIXME. _bodyA->angularVelocity( Vec3f(0.0f) ); }