//
//	Part of H2MoCap
//
//	This code examines all animation frames of a Heretic 2 player model,
//	based on a pre-defined description of the estmated model skeleton,
//	to determine the average length of each of the bones in the skeleton
//	and the standard deviation of bone length. This is intended to verify
//	that the estimated skeleton is roughly correct, and to quantify the
//	bone lengths which are not part of the estimate.
//
//	By Chris Burke
//	serotonin@earthlink.net
//
//	Date			Who		Description
//	----------		---		-------------------------------------------------------
//	07/01/1999		CJB		Coded
//	12/08/1999		CJB		Simplified trig calculations; all angles now in range
//							0..2*pi--, reversed some signs to make calculations
//							consistent.

#include	<stdio.h>
#include	<stdlib.h>
#include	<math.h>
#include	<string.h>
#include	<stdio.h>

#include	"CondComp.h"
#include	"FlexModel.h"
#include	"h2BitMaps.h"
#include	"mocap.h"
#include	"bones.h"
#include	"MatrixMath.h"

#if		(__CAPTURE_MOTION__ || __FIND_BONE_LENGTHS__)

#define	__LOCAL_DEBUG_STUFF__	0

extern void	quit( char*, int);
extern void StaffToSword( fmheader_t* pModelHeader, frame_t* pFrame, refskel_t* pRef, vec3dbl_t* pTranslate, vec3dbl_t* pRotate ); 
extern void HeadToHood( fmheader_t* pModelHeader, frame_t* pFrame, refskel_t* pRef, vec3dbl_t* pTranslate, vec3dbl_t* pRotate ); 
extern void ChestToRobe( fmheader_t* pModelHeader, frame_t* pFrame, refskel_t* pRef, vec3dbl_t* pTranslate, vec3dbl_t* pRotate );

double		boneLength[ BONE_NUM_BONES ];		//	Accumulates bone length for each bone, for average
double		boneSigma[ BONE_NUM_BONES ];		//	Accumulates length - avg, squared, for std. dev

#if	(__MODEL_IS_CORVUS__ || __MODEL_IS_KIERA__)
#include	"corvusmocap.h"
#elif	__MODEL_IS_HARPY__
#include	"harpymocap.h"
#endif


//
//	Normalize an angle to the range 0..2*pi--
//
double
AngNorm( double theta )
{
	while ( theta >= 2.*PI )
		theta -= 2.*PI;
	while ( theta <= -2.*PI )
		theta += 2.*PI;
	return	theta;
}


//
//	Given a pointer to a frame and a pointer to a locus,
//	calculate the (x, y, z) point corresponding to the
//	locus (that is, average the coordinates of all points
//	in the locus)
//
vec3dbl_t*
LocusAverage( vec3dbl_t* pRV, frame_t* pFrame, refskel_t* pRef, mocap_locus_t* pLocus )
{
	int		i, vert, nPoints;

	pRV->c[0] = 0.0;			//	X
	pRV->c[1] = 0.0;			//	Y
	pRV->c[2] = 0.0;			//	Z
	nPoints = pLocus->v[0];	//	Number of points to average

	for ( i=1; i<=nPoints; i++ )
	{
		vert = pLocus->v[i];
		//	Vert is a vertex index in the frame
		pRV->c[0] += (pFrame->verts[ vert ]).v[0] * pFrame->header.scale[0] + pFrame->header.translate[0];
		pRV->c[1] += (pFrame->verts[ vert ]).v[1] * pFrame->header.scale[1] + pFrame->header.translate[1];
		pRV->c[2] += (pFrame->verts[ vert ]).v[2] * pFrame->header.scale[2] + pFrame->header.translate[2];
	}
	pRV->c[0] /= (double)nPoints;
	pRV->c[1] /= (double)nPoints;
	pRV->c[2] /= (double)nPoints;

	return	pRV;
}


//
//	Given a pointer to a frame, calculate the key points of the frame
//	using joint loci, y-loci, and z-loci, and store them in the
//	specified arrays.
//
void
GetFrameKeyPoints( frame_t*	pFrame, refskel_t* pRef, vec3dbl_t* pJoints, vec3dbl_t* pYReference, vec3dbl_t* pZReference )
{
	int		jointIdx, boneIdx;

	for ( jointIdx = 0; jointIdx < JOINT_NUM_JOINTS; jointIdx++ )
	{
		LocusAverage( &(pJoints[ jointIdx ]), pFrame, pRef, &loci[ jointIdx ] );
	}

	for ( boneIdx = 0; boneIdx < BONE_NUM_BONES; boneIdx++ )
	{
		LocusAverage( &(pYReference[ boneIdx ]), pFrame, pRef, &y_loci[ boneIdx ] );
		LocusAverage( &(pZReference[ boneIdx ]), pFrame, pRef, &z_loci[ boneIdx ] );
	}

}

#if	__LOCAL_DEBUG_STUFF__
//	Debug routine to print actual and reconstructed point coordinates
void
DebugPrintPoint( double x, double y, double z, double kx1, double kx0, double kz1, double kz0 )
{
	printf("Actual:reconstructed X,Y,Z  %6.3f:%6.3f  %6.3f:%6.3f  %6.3f:%6.3f\n",
		x, kx1*y+kx0, y, y, z, kz1*y+kz0 );
}
#endif

//
//	Given two points that define the ends of a bone, with the first
//	point defining the end of the bone closest to the root (hip joint),
//	and a 3rd point that is not colinear with the first two, calculate
//	the point on the axis of the bone at which a line through the
//	3rd point and perpendicular to the axis will intersect the axis.
//
//	The basic math is to calculate the equation of the axis in the
//	form x=f(y), z=f(y), and the equation of a sphere surrounding
//	the 3rd point in the form r**2 = (x-x3)**2 + (y-y3)**2 + (z-z3)**2,
//	and then substitute the axis equation into the sphere equation
//	and use the 1st derivative to find the value of Y producing the
//	minimum value of r**2. By definition, a sphere of this radius
//	is tangent to the axis, and from this Y we can calculate the
//	X and Z of the point of intersection by substituting into the
//	equation of the axis.
//
void
FindBoneAxisIntersection( vec3dbl_t* pCross, vec3dbl_t* pRoot, vec3dbl_t* pEnd, vec3dbl_t* pOrbit )
{
	double	cx1, cz1;
	double	cx0, cz0;
	double	x1, y1, z1;
	double	x2, y2, z2;
	double	x3, y3, z3;
	double	xr, yr, zr;

	//	End closest to hip
	x1 = pRoot->c[0];
	y1 = pRoot->c[1];
	z1 = pRoot->c[2];
	//	Far end
	x2 = pEnd->c[0];
	y2 = pEnd->c[1];
	z2 = pEnd->c[2];
	//	Orbit point
	x3 = pOrbit->c[0];
	y3 = pOrbit->c[1];
	z3 = pOrbit->c[2];

	//	Useful format of equation of the line:
	//	(x-x1)(y2-y1)=(y-y1)(x2-x1)
	//	(y-y1)(z2-z1)=(z-z1)(y2-y1)
	//	(z-z1)(x2-x1)=(x-x1)(z2-z1)
	
	if ( y2 == y1 )
		y2 = y1 + 1.e-10;

	//	Replace X with f(y)
	//	x = (y-y1)(x2-x1)/(y2-y1) + x1
	//	x = ((x2-x1)/(y2-y1))y + x1 - y1(x2-x1)/(y2-y1)
	cx1 = (x2-x1) / (y2-y1);				//	x = cx1*y + cx0
	cx0 = x1 - y1*(x2-x1)/(y2-y1);
	//	Replace Z with f(y)
	//	z = (y-y1)(z2-z1)/(y2-y1) + z1
	//	z = ((z2-z1)/(y2-y1))y + z1 - y1(z2-z1)/(y2-y1)
	cz1 = (z2-z1) / (y2-y1);				//	z = cz1*y + cz0
	cz0 = z1 - y1*(z2-z1)/(y2-y1);

	//	Now any point on the line is given by:
	//	( cx1*y + cx0, y, cz1*y + cz0 )

#if	__LOCAL_DEBUG_STUFF__
	DebugPrintPoint( x1, y1, z1, cx1, cx0, cz1, cz0 );
	DebugPrintPoint( x2, y2, z2, cx1, cx0, cz1, cz0 );
#endif

	//	Substitute into equation of sphere and take first derivative
	//	kR2 = (x-x3)**2 + (y-y3)**2 + (z-z3)**2
	//	kR2 = (cx1*y + cx0 - x3)**2 + (y-y3)**2 + (cz1*y + cz0 - z3)**2
	//	kR2 = (cx1**2 + 1 + cz1**2)(y**2) + (2cx1(cx0-x3) - 2y3 + 2cz1(cz0-z3))y + kFoo
	//	dkR2/dy = 2(cx1**2+1+cz1**2)y + (2cx1(cx0-x3) - 2y3 + 2cz1(cz0-z3))
	//	0 = 2(cx1**2+1+cz1**2)y + (2cx1(cx0-x3) - 2y3 + 2cz1(cz0-z3))
	//	0 = (cx1**2+1+cz1**2)y + (cx1(cx0-x3) - y3 + 2z1(cz0-z3))

	yr = -(cx1*(cx0-x3) - y3 + cz1*(cz0-z3)) / (cx1*cx1+1+cz1*cz1);

	//	Substitute back into equation of line
	xr = (cx1 * yr) + cx0;
	zr = (cz1 * yr) + cz0;

#if	__LOCAL_DEBUG_STUFF__
	DebugPrintPoint( xr, yr, zr, cx1, cx0, cz1, cz0 );
#endif

	//	Return the answer
	pCross->c[0] = xr;
	pCross->c[1] = yr;
	pCross->c[2] = zr;
}

//
//	Calculate a rotational angle about the Z axis in radians,
//	in the range 0..2pi, given an x and y coordinate. If both
//	x and y are zero, zero is returned.
//
static double
GetRotAngle( double x, double y )
{
	//	WARNING: ATAN2 IS UNDEFINED FOR (0,0)
	double	rv;
	if ( (x==0.) && (y==0.) )
		return	0.;						//	WARNING: ATAN2 IS UNDEFINED FOR (0,0) - FORCE
	rv = atan2( y, x );					//	range is -pi..+pi
	if ( rv < 0. ) rv += (2.*PI);		//	return range is 0..2pi
	return	rv;
}

//
//	Given a pointer to a frame, calculate the positions of all the
//	key points, and then, working out from the hips, determine the
//	displacement and angle of rotation of each part of the skeleton
//	for this frame. Note that in the default position, all bones
//	are aligned along the X axis with the hips at (0,0,0) and
//	joints at a position on the X-axis given by the lengths of all
//	the bones between this joint and the hip.
//
//	NOTE: THIS IS JUST A DUMMY VERSION OF THE FUNCTION. IT DETERMINES
//	THE ABSOLUTE DISPLACEMENT AND LOCATION OF THE STAFF (AS IF THE
//	STAFF WERE THE HIPS). THIS ALLOWS A SIMPLE EXPERIMENT IN WHICH
//	THE STAFF GEOMETRY IS REPLACED BY A SWORD.
//
void
FindFrameMotion( fmheader_t* pModelHeader, frame_t* pFrame, refskel_t* pRef, vec3dbl_t* pTranslate, vec3dbl_t* pRotate )
{
	vec3dbl_t	jointPos[ JOINT_NUM_JOINTS ];
	vec3dbl_t	boneY[ BONE_NUM_BONES ];
	vec3dbl_t	boneZ[ BONE_NUM_BONES ];
	vec3dbl_t	crossPoint, refPoint, endPoint, pivotPoint;
	vec3dbl_t	thetas;
	double		thetaX, thetaY, thetaZ;
	int			bone;

	GetFrameKeyPoints( pFrame, pRef, &jointPos[0], &boneY[0], &boneZ[0] );

	for ( bone = 0; bone < BONE_NUM_BONES; bone++ )
	{

		//	Copy endpoints of this bone
		endPoint   = jointPos[ boneEnds[ bone ].n[1] ];
		pivotPoint = jointPos[ boneEnds[ bone ].n[0] ];

		//	WARNING: NON-HIERARCHICAL VERSION
		//	ALL BONE POSITIONS AND ROTATIONS ARE CALCULATED
		//	RELATIVE TO THE ORIGIN RATHER THAN RELATIVE TO
		//	THE NEXT JOINT UP THE HIERARCHY

		//	Calculate offset of joint from origin
		*pTranslate = pivotPoint;

		//	Copy the y-axis reference point for this bone (for x-axis
		//	rotation calculation)
		refPoint = boneY[ bone ];
//		//	TEST: See if intersection stuff finds pivot given pivot
//		refPoint = pivotPoint;

		//	Find where y-axis rotational indicator intersects bone
		FindBoneAxisIntersection(
			&crossPoint, &pivotPoint, &endPoint, &refPoint
		);

		//	Translate the end of the y-axis rotational indicator to
		//	intersect the bone at the joint closest to the hip
		refPoint.c[0] += (pivotPoint.c[0] - crossPoint.c[0]);
		refPoint.c[1] += (pivotPoint.c[1] - crossPoint.c[1]);
		refPoint.c[2] += (pivotPoint.c[2] - crossPoint.c[2]);

#if	__LOCAL_DEBUG_STUFF__
		//	Double-Check: Find where revised y-axis rotational indicator
		//	(refpoint) intersects bone - it should be at pivotPoint
		{
			FindBoneAxisIntersection(
				&crossPoint, &pivotPoint, &endPoint, &refPoint
			);
			crossPoint = crossPoint;
		}
#endif

		//	Translate both the bone and the y-axis rotational indicator
		//	to the origin. Since the joint nearest the hip is moved to
		//	the origin, and both the bone and the rotational indicator
		//	are defined relative to this joint, only the endpoints are
		//	actually translated. Also, convert both to unit vectors.
		M3DUnTranslate( &refPoint, &pivotPoint );
		M3DUnitVector(  &refPoint, &refPoint );
		M3DUnTranslate( &endPoint, &pivotPoint );
		M3DUnitVector(  &endPoint, &endPoint );

#if	__LOCAL_DEBUG_STUFF__
		//	Triple-Check: Calculate angle formed by pivot-end and pivot-ref
		//	Since refPoint and endPoint are both unit vectors, we can
		//	calculate the angle between then using:
		//	pi/2 = phi + theta + gamma
		//	gamma == theta
		//	cos theta = 1/2(dist between refPoint and endPoint) / 1.0
		//	phi = pi - 2*acos(dist/2)
		{
			double	d, phi, theta, foo;
			d = M3DLength( &refPoint, &endPoint);
			theta = acos(d/2.);
			phi = PI - 2.*theta;
			phi = AngNorm(phi);
			foo = theta*(180./PI);
			foo = foo;
		}
#endif
		
		//	Rotate bone end and Y-axis reference point so that bone points
		//	along X axis. From this we determine the angles of rotation about
		//	the Z and Y axes, and set up to determine the angle of rotation
		//	about the X axis.
		//
		//	The bone (already translated to the origin) is rotated about
		//	the Y (vertical) axis to lie in the XY. plane, and then about the
		//	Z (depth) axis to lie along the X axis

		thetaY = AngNorm( GetRotAngle( endPoint.c[0], endPoint.c[2] ) );			//	x, z

		thetas.c[0] = 0.;
		thetas.c[1] = -thetaY;
		thetas.c[2] = 0.;
		M3DRotate( &endPoint, &thetas );
		M3DRotate( &refPoint, &thetas );

		//	Now the Z coordinate of the endpoint is zero.
		//	Rotation around y,x (z axis) in our coordinate system
		//	would align the bone with the y axis and force x=0.
		//	But if we subtract PI/2 from the resulting angle,
		//	we will align the bone along the X axis and force y=0.

//		thetaZ = 2.*PI - GetRotAngle( endPoint.c[0], endPoint.c[1] );
//		while (thetaZ >= 2.*PI)
//			thetaZ -= 2.*PI;
//		while (thetaZ <= -2.*PI)
//			thetaZ += 2.*PI;
		thetaZ = AngNorm( GetRotAngle( endPoint.c[1], endPoint.c[0] ) - PI/2. );	//	y, x, adjusted

		thetas.c[0] = 0.;
		thetas.c[1] = 0.;
		thetas.c[2] = -thetaZ;
		M3DRotate( &endPoint, &thetas );
		M3DRotate( &refPoint, &thetas );

		//	Now the Y and Z coordinates of the endpoint are zero,
		//	and the X coordinate of the reference point is zero.

		//	Now we know two of the rotation angles. The bone starts
		//	at the origin and points along the X axis. The Y axis
		//	reference point should have X=0. We can now calculate
		//	thetaX, the remaining angle, based on the other two
		//	coordinates of the y axis reference point.
		//	Rotation around z,y (x axis) in our coordinate system
		//	would align the bone with the z axis and force y=0.
		//	But if we subtract PI/2 from the resulting angle,
		//	we will align the bone along the Y axis and force z=0.

//		thetaX = -GetRotAngle( refPoint.c[2], refPoint.c[1] );
//		thetaX += PI/2.;
//		thetaX =  -thetaX;			//	Correction factor
//		while (thetaX >= 2.*PI)
//			thetaX -= 2.*PI;
//		while (thetaX <= -2.*PI)
//			thetaX += 2.*PI;
		thetaX = AngNorm( GetRotAngle( refPoint.c[2], refPoint.c[1] ) - PI/2. );	//	z, y, adjusted

//#if	__LOCAL_DEBUG_STUFF__
		thetas.c[0] = -thetaX;
		thetas.c[1] = 0.;
		thetas.c[2] = 0.;
		M3DRotate( &endPoint, &thetas );
		M3DRotate( &refPoint, &thetas );

		//	Now the X and Z coordinates of the endpoint are zero,
		//	and the X and Z coordinates of the reference point
		//	are also zero.
//#endif

		//	Save the rotation information
		pRotate->c[0] = thetaX;
		pRotate->c[1] = thetaY;
		pRotate->c[2] = thetaZ;

		//	Setup for next bone
		++pRotate;
		++pTranslate;
	}
}

//
//	This routine extracts motion capture data from the model
//	by examining the movement of pre-specified joints and bones
//	in 3D space.
//

void
CaptureMotion( fmheader_t* pModelHeader, int nFrames, frame_t* pFirstFrame, refskel_t* pFirstRef )
{
	int			i, j;
	frame_t*	pFrame;
	refskel_t*	pRef;
	vec3dbl_t*	pBonesMoCap;
	vec3dbl_t*	pMoCap;

	//	Allocate memory array to store motion capture data
	if ( !(pBonesMoCap = malloc( nFrames*2*BONE_NUM_BONES*sizeof(vec3dbl_t) )) )
		quit("Can't allocate memory for motion capture", -1);
	pMoCap = pBonesMoCap;

	pFrame = pFirstFrame;
	pRef   = pFirstRef;
	for ( i=0; i< nFrames; i++ )
	{
		FindFrameMotion( pModelHeader, pFrame, pRef, pMoCap+0, pMoCap+BONE_NUM_BONES );
#if	__MODEL_IS_CORVUS__ || __MODEL_IS_KIERA__
#if	__INSTALL_HOOD__
		//	NOTE: Corvus hood is a body part substitution; Kiera is different
		HeadToHood( pModelHeader, pFrame, pRef, pMoCap+0, pMoCap+BONE_NUM_BONES ); 
#endif
#endif
#if	__MODEL_IS_KIERA__
#if	__INSTALL_ROBE__
		//	Chest part of Kiera robe
		ChestToRobe( pModelHeader, pFrame, pRef, pMoCap+0, pMoCap+BONE_NUM_BONES ); 
#endif
#endif
#if	__INSTALL_SWORD__
		StaffToSword( pModelHeader, pFrame, pRef, pMoCap+0, pMoCap+BONE_NUM_BONES ); 
#endif
		pMoCap += 2*BONE_NUM_BONES;
		pFrame = (frame_t*)((byte*)pFrame + pModelHeader->framesize);
		++pRef;
	}
#if	__WRITE_MOCAP_DATA__
	{
		//	Write all the motion capture data for Corvus to a file.
		//	File format is described in mocap.h. Note that this is
		//	a non-heirarchical format, but that enough information
		//	is stored to construct a heirarchical representation
		//	given knowledge of the skeleton.
		FILE*			fp1;
		mocaphdr_t		hdr;
		framemotion_t	cap;
		if ( !(fp1 = fopen( "mocap.bin", "wb" )) )
			quit( "Can't open 'mocap.bin'.", -1 );
		//	Write header
		hdr.framecnt = nFrames;
		hdr.version  = CURRENT_MOCAP_VERSION;
		fwrite( &hdr, sizeof(mocaphdr_t), 1, fp1);
		pFrame = pFirstFrame;
		pMoCap = pBonesMoCap;
		for ( i = 0; i < nFrames; i++ )
		{
			//	Write frame name
			memset( &(cap.name), 0, sizeof(cap.name) );
			strcpy( cap.name.name, pFrame->header.name );
			//	Write translation and rotation data for this frame
			for ( j=0; j<BONE_NUM_BONES; j++ )
			{
				cap.transXYZ[j].c[0] = pMoCap[0+j].c[0];
				cap.transXYZ[j].c[1] = pMoCap[0+j].c[1];
				cap.transXYZ[j].c[2] = pMoCap[0+j].c[2];
			}
			for ( j=0; j<BONE_NUM_BONES; j++ )
			{
				cap.rotXYZ[j].c[0] = pMoCap[BONE_NUM_BONES+j].c[0];
				cap.rotXYZ[j].c[1] = pMoCap[BONE_NUM_BONES+j].c[1];
				cap.rotXYZ[j].c[2] = pMoCap[BONE_NUM_BONES+j].c[2];
			}
			fwrite( &cap, sizeof(framemotion_t), 1, fp1);
			pMoCap += 2*BONE_NUM_BONES;
			pFrame = (frame_t*)((byte*)pFrame + pModelHeader->framesize);
		}
		fclose( fp1 );
		printf("    wrote 'mocap.bin'\n");
		i = i;
	}
#endif
	free( pBonesMoCap );
}

//
//	This routine figures out average bone length by examining the
//	joint loci in all frames of animation.
//

void
FindBoneLengths( fmheader_t* pModelHeader, int nFrames, frame_t* pFirstFrame, refskel_t* pFirstRef )
{
	int			frameIdx, boneIdx, endIdx, locusIdx;
	vec3dbl_t	temp[2];
	double		dist;
	frame_t*	pFrame;
	refskel_t*	pRef;

	//	For each frame in the model, we want to calculate the average distance
	//	between the ends of each bone. The coordinates of the ends of each bone
	//	are calculated as the average of all the points in the locus of that end.
	//	A first pass calculates average distance. A 2nd pass calculates standard
	//	deviation as a measure of how accurate the joint definitions are.

	//	Pass 1: Calculate average bone lengths
	//	For all defined bones...
	for ( boneIdx = 0; boneIdx < BONE_NUM_BONES; boneIdx++ )
	{
		//	Zero the calculated average length of this bone
		boneLength[ boneIdx ] = 0.0;
		//	For all frames of animation (all known poses of the model)
		pFrame = pFirstFrame;
		pRef   = pFirstRef;
		for ( frameIdx = 0; frameIdx < nFrames; frameIdx++ )
		{
#if	__EXCLUDE_PARTFLY__
		  if ( strcmp(pFrame->header.name, "partfly") )
		  {
#endif
			//	One length for the bone is calculated for each frame.
			//	To do this, we use the locus of the joints bounding
			//	the bone.
			for ( endIdx = 0; endIdx < 2; endIdx++ )
			{
				//	For both ends of each bone...
				//	Figure out which locus to use for this end
				locusIdx = (boneEnds[ boneIdx ]).n[ endIdx ];
				LocusAverage( &(temp[ endIdx ]), pFrame, pRef, &(loci[ locusIdx ]) );
				if (loci[locusIdx].v[1] == -(H2CORVUS_LF + 1))
					endIdx = endIdx;
			}
			//	The locations of the joints have been estimated for this frame.
			//	Now calculate the distance, in this frame, between the two
			//	joints bounding this bone, as an estimate of bone length.
			dist =
				sqrt(
					pow( (temp[1]).c[0] - (temp[0]).c[0], 2.0) +
					pow( (temp[1]).c[1] - (temp[0]).c[1], 2.0) +
					pow( (temp[1]).c[2] - (temp[0]).c[2], 2.0)
				);
			//	Add together all length estimates for this bone (the
			//	estimate from each frame)
			boneLength[ boneIdx ] += dist;
#if	__EXCLUDE_PARTFLY__
		  }
#endif
			//	Advance to the next frame
			pFrame = (frame_t*)((byte*)pFrame + pModelHeader->framesize);
			++pRef;
		}
		//	A bone length estimate has been calculated for each frame.
		//	Now divide by the number of estimates (the number of frames),
		//	to get the average estimate of bone length for this bone.
		boneLength[ boneIdx ] /= nFrames;
		//	Do the same for the next bone
	}

	//	Pass 2: Calculate bone length standard deviation for each bone.
	//	This gives a good idea of how accurate the bone joint location
	//	estimates are (assuming the right bones have been chosen and
	//	only the joint loci are in question)
	//	For all defined bones...
	for ( boneIdx = 0; boneIdx < BONE_NUM_BONES; boneIdx++ )
	{
		//	Zero the calculated Q of this bone
		boneSigma[ boneIdx ] = 0.0;
		//	For all frames of animation (all known poses of the model)
		pFrame = pFirstFrame;
		pRef   = pFirstRef;
		for ( frameIdx = 0; frameIdx < nFrames; frameIdx++ )
		{
#if	__EXCLUDE_PARTFLY__
		  if ( strcmp(pFrame->header.name, "partfly") )
		  {
#endif
			//	One length for the bone is calculated for each frame.
			//	To do this, we use the locus of the joints bounding
			//	the bone.
			for ( endIdx = 0; endIdx < 2; endIdx++ )
			{
				//	For both ends of each bone...
				//	Figure out which locus to use for this end
				locusIdx = (boneEnds[ boneIdx ]).n[ endIdx ];
				LocusAverage( &(temp[ endIdx ]), pFrame, pRef, &(loci[ locusIdx ]) );
			}
			//	The locations of the joints have been estimated for this frame.
			//	Now calculate the distance, in this frame, between the two
			//	joints bounding this bone, as an estimate of bone length.
			dist =
				sqrt(
					pow( (temp[1]).c[0] - (temp[0]).c[0], 2.0) +
					pow( (temp[1]).c[1] - (temp[0]).c[1], 2.0) +
					pow( (temp[1]).c[2] - (temp[0]).c[2], 2.0)
				);
			//	Calculate the squared variance from the average length of
			//	this bone, for this frame.
			dist = pow( dist - boneLength[ boneIdx ], 2.0 );
			//	Add together all squared variances for this bone (the
			//	estimate from each frame)
			boneSigma[ boneIdx ] += dist;
#if	__EXCLUDE_PARTFLY__
		  }
#endif
			//	Advance to the next frame
			pFrame = (frame_t*)((byte*)pFrame + pModelHeader->framesize);
			++pRef;
		}
		//	A squared variance has been calculated for each frame.
		//	Now divide by the number of estimates (the number of frames),
		//	to get the average squared variance.
		boneSigma[ boneIdx ] /= nFrames;
		//	Now divide by the average length, squared, to get std. deviation
		boneSigma[ boneIdx ] /= pow( boneLength[ boneIdx ], 2.0 );
		//	Do the same for the next bone
	}

	//	Display the results
	for ( boneIdx = 0; boneIdx < BONE_NUM_BONES; boneIdx++ )
		printf(
			"    %20s: avg. length %8.4f, std.dev %8.4f\n",
			boneNames[ boneIdx ], boneLength[ boneIdx ], boneSigma[ boneIdx ]
		);

	boneIdx = boneIdx;		//	DEBUG
}

#endif