Counter clockwise (right hand rule with thumb on arrow side) is positive.
Multiply two rotation matrices together and work out what the angles α and θ are.
Rotate about axis a by angle θ.
Axis of rotation = normalize(target vector × orig vector)
norm(af×a0)=norm((1,1,1)×(0,0,1))=(1,−1,0)/√12+(−1)2+0=(√1/2,−√1/2,0)Angle of rotation = cos−1(unit target dot unit orig)
Substitute into the matrix:
Cross product of two vectors:
[a1a2a3]×[b1b2b3]=[a2b3−a3b2a3b1−a1b3a1b2−a2b1]Multiplication:
q1q2=(w1,v1)×(w2,v2)=(w1w2−v1×v2,(Scalar)w1v2+w2v1+v1×v2)(Vector)void renderJoints(JOINT* joint, float*data, float scale) {
glPushMatrix();
// translate
if ( joint->parent == NULL ) { // is Root
glTranslatef( data[0] * scale, data[1] * scale, data[2] * scale );
} else {
glTranslatef( joint->offset.x*scale, joint->offset.y*scale, joint->offset.z*scale );
}
// rotate
for ( uint i=0; i<joint->channels.size(); i++ ) {
CHANNEL *channel = joint->channels[i];
if ( channel->type == X_ROTATION )
glRotatef( data[channel->index], 1.0f, 0.0f, 0.0f );
else if ( channel->type == Y_ROTATION )
glRotatef( data[channel->index], 0.0f, 1.0f, 0.0f );
else if ( channel->type == Z_ROTATION )
glRotatef( data[channel->index], 0.0f, 0.0f, 1.0f );
}
// end site
if ( joint->children.size() == 0 ) {
renderSphere(joint->offset.x*scale, joint->offset.y*scale, joint->offset.z*scale,0.07);
}
if ( joint->children.size() == 1 ) {
JOINT * child = joint->children[ 0 ];
renderSphere(child->offset.x*scale, child->offset.y*scale, child->offset.z*scale,0.07);
}
if ( joint->children.size() > 1 ) {
float center[ 3 ] = { 0.0f, 0.0f, 0.0f };
for ( uint i=0; i<joint->children.size(); i++ )
{
JOINT * child = joint->children[i];
center[0] += child->offset.x;
center[1] += child->offset.y;
center[2] += child->offset.z;
}
center[0] /= joint->children.size() + 1;
center[1] /= joint->children.size() + 1;
center[2] /= joint->children.size() + 1;
renderSphere(center[0]*scale, center[1]*scale, center[2]*scale,0.07);
for ( uint i=0; i<joint->children.size(); i++ )
{
JOINT * child = joint->children[i];
renderSphere(child->offset.x*scale, child->offset.y*scale, child->offset.z*scale,0.07);
}
}
// recursively render all joints
for ( uint i=0; i<joint->children.size(); i++ )
{
renderJointsGL( joint->children[i], data, scale );
}
glPopMatrix();
}
a = axis of rotation
r = distance vector from joint to end effector
m = number of effectors
ve = velocity of end effector in one step
ωe = angular velocity of end effector in one step
J=[a1×r1a2×r2…am×rma1a2…am]Solving: [veωe]=J[Δθ1⋮Δθm][Δθ1⋮Δθm]=J+[veωe]θt+1=θt+Δθ
Or see gradient descent (Jacobian Transpose).
Hooke’s Law + Damping: