Fix to DOF Record from Sasa Bistroviae.

src/osgPlugins/flt/flt2osg.cpp

@@ -636,21 +636,115 @@ osg::Group* ConvertFromFLT::visitDOF(osg::Group& osgParent, DofRecord* rec)
     // below is DOF code submitted by Sasa Bistrovic
     SDegreeOfFreedom* p_data = rec->getData();
 
+	//this first transfrom is the transformation fo the local coordinate system in DOF to global;
+	//that one is "fixed for all times"
+	//the second transform describes transfromation relative to local coordinate system
+
+	//first we need to setup local-to-global transform:
+
+	//the translation matrix:
+	osg::Matrix l2g_translation;
+
+	//translation vector
+	osg::Vec3 origin_translation(p_data->OriginLocalDOF.x(), p_data->OriginLocalDOF.y(), p_data->OriginLocalDOF.z());
+	origin_translation *= _unitScale;//of course, has to be scaled to units used
+
+	//so we make translation matrix
+	l2g_translation.makeTranslate(origin_translation);
+
+	//now we need to construct rotation matrix that describes how is local coordinate system
+	//rotatoed in global:
+	osg::Matrix l2g_rotation;
+
+	//we make two normalized osg::Vec3s so we can cross-multiply them ....
+
+	//first we have "point on x axis", i.e. what the DOF local looks like in global
+	osg::Vec3 point_on_x_axis(p_data->PointOnXaxis.x(), p_data->PointOnXaxis.y(), p_data->PointOnXaxis.z());
+	point_on_x_axis *= _unitScale;
+	point_on_x_axis -= origin_translation;//we need to offset it for the origin
+
+	//second, we have "point in xy plane", so x ^ y gives us local z vector
+	osg::Vec3 point_in_xy_plane(p_data->PointInXYplane.x(), p_data->PointInXYplane.y(), p_data->PointInXYplane.z());
+	point_in_xy_plane *= _unitScale;
+	point_in_xy_plane -= origin_translation;//also offset it
+
+	//just in case, normalize them:
+	point_on_x_axis.normalize();
+
+	//now, local x in global is [1 0 0 1] * rotation_matrix, so we can directly write down first row
+	l2g_rotation.ptr()[0] = point_on_x_axis.x(); //that is mat[0][0]
+	l2g_rotation.ptr()[1] = point_on_x_axis.y(); //that is mat[0][1]
+	l2g_rotation.ptr()[2] = point_on_x_axis.z(); //that is mat[0][2]
+
+	
+	//the cross product of x ^point_in_local_xy_plane gives local z
+	osg::Vec3 z_transformed = point_on_x_axis ^ point_in_xy_plane;
+	z_transformed.normalize();//we have to normalize it, and than we can directly writ it down to rotation matrix
+	//we can write it down to matrix directly:
+	l2g_rotation.ptr()[8] = z_transformed.x(); //that is mat[2][0]
+	l2g_rotation.ptr()[9] = z_transformed.y(); //that is mat[2][1]
+	l2g_rotation.ptr()[10] = z_transformed.z(); //that is mat[2][2]
+
+	//now that we have x and z, we can get y = z ^ x
+	osg::Vec3 y_transformed = z_transformed ^ point_on_x_axis;
+	//and just in case, we can normalize it:
+	// y_transformed.normalize();
+
+	//this goes to rotation matrix:
+	l2g_rotation.ptr()[4] = y_transformed.x(); //that is mat[1][0]
+	l2g_rotation.ptr()[5] = y_transformed.y(); //that is mat[1][0]
+	l2g_rotation.ptr()[6] = y_transformed.z(); //that is mat[1][0]
+
+
+	//now we have actually the second "PUT" transformation, and the first is inverse
+	osg::Matrix second_put = l2g_rotation * l2g_translation;
+
+	//and we take invers of it since it has to be done at the end, see FLT spec
+	osg::Matrix the_first = osg::Matrix::inverse(second_put);
+
+	//now the vertices underneath DOF are "in local system" and we do TRS, as in record
+
+	//we setup matrix for this "second transformation"
+
     osg::Matrix mat;
 
-    mat.makeTranslate(_unitScale*p_data->dfX._dfCurrent,
+	//first we write down translation
+	mat.makeTranslate(_unitScale*p_data->dfX._dfCurrent,
                       _unitScale*p_data->dfY._dfCurrent,
                       _unitScale*p_data->dfZ._dfCurrent);
-
+	
+	//now we read roll, pitch and yaw angles and convert them to radians
     float roll_rad = osg::inDegrees(p_data->dfRoll._dfCurrent);
     float pitch_rad = osg::inDegrees(p_data->dfPitch._dfCurrent);
     float yaw_rad = osg::inDegrees(p_data->dfYaw._dfCurrent);
 
-    osg::Matrix mat_rot = osg::Matrix::rotate(-yaw_rad, pitch_rad,roll_rad);
+	osg::Matrix local_rotation;
 
-    mat.preMult(mat_rot);
+	//the rotation part: the flt spec says this order:
+	osg::Matrix mat_pitch = osg::Matrix::rotate(pitch_rad, 1.0, 0.0, 0.0);
+	osg::Matrix mat_roll = osg::Matrix::rotate(roll_rad, 0.0, 1.0, 0.0);	
+	osg::Matrix mat_yaw = osg::Matrix::rotate(yaw_rad, 0.0, 0.0, 1.0);
+	
+	local_rotation.preMult(mat_pitch);
+	local_rotation.preMult(mat_roll);
+	local_rotation.preMult(mat_yaw);
 
-    transform->setMatrix(mat);
+	mat.preMult(local_rotation);
+
+	//and finally, we have to put up a scale as read from record
+	osg::Matrix scale;
+	scale.makeScale(p_data->dfXscale._dfCurrent, 
+		p_data->dfYscale._dfCurrent, 
+		p_data->dfZscale._dfCurrent);
+
+	mat.preMult(scale);
+
+	//we have everything ready now: the first transformation is followed by "local transformation" and than "put back"
+
+	the_first.postMult(mat);
+	the_first.postMult(second_put);
+
+	transform->setMatrix(the_first);
 
     return transform;        
 }