Quaternion based angular constraints and verlet integration

Hello All I've spent nearly two weeks working on this problem now in one guise or another and am (literally) tearing my hair out (I have big pile of it on the desk here - but you probably didn't need to know that) I am writing a simulation of bird flight in DX8 and VB6. At its heart, it is a mass/spring simulation based upon the Jacobsen verlet integration system. I'm trying to impliment an angular constraint - a sprung joint that is to angles what springs are to distances. I did get some good suggestions in #gamedev but none of them solved my problem yet So, I have three masses, A,B and C - connected with two springs AB and BC - B is also a constraining joint I want to maintain the angle ABC - my thinking is :- Record the 'Resting' Quaternion do 'Measure' the 'current' quaternion derive the error (difference between the current and rest quaternions) distrubute the error as rotations of A and C about B (in inverse proportion to their mass * radius) loop This *nearly* works - I've posted a video of the actual problem at http://www.quicksnooker.com/crazy.avi A few words of explaination are required The program starts and the system is stable. Mass A is on the right, B is the 'elbow' and C is at the bottom. A second or two in I click near mass A to drag it towards the mouse cursor. Everything responds correctly (given that its a fairly 'springy' joint) 2 or 3 seconds later - it all goes pear shaped. For the purposes of the video - and to keep things simple - the three masses are equal, and the moment arms are the same length. Heres the code:- Record resting quaternion at 'startup'


  D3DXQuaternionRotationAxis TempJoint(j).restQuat, VecCrossProduct(abn, bcn), ArcCos(VecDotProduct(abn, bcn))

Then, the main loop goes a bit like this:-


function Cycle()

   	Verlet()
                    
	For o = 1 To 7                         
                        
       		.SimpleStretchSprings 'Then Exit For
                       
        Next o
                        
                        
        .ProcessJointsQuat MainViewport
                    
        .drawscene

        
end function

And the ProcessJointsQuat Function looks like this:-


Friend Function ProcessJointsQuat(vp As viewport)


Dim cq As D3DQUATERNION
Dim q As D3DQUATERNION
Dim m As D3DMATRIX
Dim j As Integer

Dim a As D3DVECTOR
Dim b As D3DVECTOR
Dim c As D3DVECTOR

Dim ab As D3DVECTOR
Dim bc As D3DVECTOR


Dim abn As D3DVECTOR
Dim bcn As D3DVECTOR

Dim ir As D3DQUATERNION

Dim amr As Single
Dim cmr As Single
Dim tmr As Single

Dim oq As Single


Dim Correction As D3DQUATERNION
Dim CorrectA As D3DQUATERNION
Dim CorrectC As D3DQUATERNION



Dim ca As Single
Dim tc As D3DVECTOR
Dim icq As D3DQUATERNION 'Inverse of the current quaternion
Dim irq As D3DQUATERNION 'inverse of the rest quaternion

Dim IdentityQuaternion As D3DQUATERNION
D3DXQuaternionIdentity IdentityQuaternion
     
Dim JointStiffness As Single
JointStiffness = 0.01

     
For j = 1 To ThisScene.Joints

    With ThisScene.Joint(j)
        
        a = ThisScene.mass(.a).P
        b = ThisScene.mass(.b).P
        c = ThisScene.mass(.c).P
       
        ab = VecSub(b, a)
        bc = VecSub(b, c)
        
        abn = VecNormalise(ab)
        bcn = VecNormalise(bc)
           
        amr = ThisScene.mass(.a).mass * D3DXVec3Length(ab)
        cmr = ThisScene.mass(.b).mass * D3DXVec3Length(bc)
        tmr = amr + cmr
           
        D3DXQuaternionRotationAxis cq, VecNormalise(VecCrossProduct(abn, bcn)), ArcCos(VecDotProduct(abn, bcn))
        
        D3DXQuaternionInverse irq, TempJoint(j).restQuat
        D3DXQuaternionInverse icq, cq
        
        'Create a correction quaternion - to restore A to its resting angle from C about B
        'correction is inverse of resting quaternion * current quaternion
        D3DXQuaternionMultiply Correction, irq, cq
                
        
        D3DXQuaternionSlerp CorrectA, IdentityQuaternion, Correction, cmr / tmr * JointStiffness 'only do a small amout of the correction at once
        
        D3DXQuaternionSlerp CorrectC, IdentityQuaternion, Correction, amr / tmr * JointStiffness
     
        
        D3DXMatrixRotationQuaternion m, CorrectA
        '
        tc = VecSub(a, b)                  'translate a so we rotate about b
        D3DXVec3TransformCoord tc, tc, m
        ThisScene.mass(.a).P = VecAdd(tc, b)   'translate back
       
       
        CorrectC.w = -CorrectC.w
        D3DXMatrixRotationQuaternion m, CorrectC
        
        tc = VecSub(c, b)
        D3DXVec3TransformCoord tc, tc, m
        ThisScene.mass(.c).P = VecAdd(tc, b)   'translate back
        
           
    End With
Next j
           

End Function

Am I barking up the wrong tree here ? Can somone tell me if my construction of the quaternion is right ? Ditto - the derivation of the error ? Is it legitimate to rotate about B - or should I be rotating about the CG of the whole system (A,B,C) - if so how much do I rotate B by ? I tried it with Axis/Angles (instead of quaternions) - and ended up with very similar problems Please help Pretty please I promise I'll learn C - Oh, and maths and I'll credit you in my game/buy you a beer/give you my firstborn child Nick [Edited by - curly1968 on August 14, 2007 12:44:18 PM]

Topic Locked