src/poser_octavioradii.c
static FLT calculateFitnessOld2(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
{
	FLT fitness = 0;
	for (size_t i = 0; i < numPairs; i++)
	{
		// These are the vectors that represent the direction for the two points.  
		// TODO: optimize by precomputing the tangent.
		FLT v1[3], v2[3], diff[3];

		v1[0] = 1;
		v2[0] = 1;
		v1[1] = tan(angles[pairs[i].index1].HorizAngle); // can be precomputed
		v2[1] = tan(angles[pairs[i].index2].HorizAngle); // can be precomputed
		v1[2] = tan(angles[pairs[i].index1].VertAngle);  // can be precomputed
		v2[2] = tan(angles[pairs[i].index2].VertAngle);  // can be precomputed

														 // Now, normalize the vectors
		normalize3d(v1, v1); // can be precomputed
		normalize3d(v2, v2); // can be precomputed

							 // Now, given the specified radii, find where the new points are
		scale3d(v1, v1, radii[pairs[i].index1]);
		scale3d(v2, v2, radii[pairs[i].index2]);

		// Cool, now find the vector between these two points
		// TODO: optimize the following two funcs into one.
		sub3d(diff, v1, v2);

		FLT distance = magnitude3d(diff);

		FLT t1 = magnitude3d(v1);
		FLT t2 = magnitude3d(v2);



		FLT estimatedDistanceBetweenPoints =

			SQUARED(radii[pairs[i].index1])
			+ SQUARED(radii[pairs[i].index2])
			- 2 * radii[pairs[i].index1] * radii[pairs[i].index2]
			* FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle)
			* FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle)
			+ FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle);


		//fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance);
		fitness += SQUARED(distance - pairs[i].KnownDistance);
	}

	return FLT_SQRT(fitness);
}
// angles is an array of angles between a sensor pair
// pairs is an array of point pairs
// radii is the guess at the radii of those angles
angles is an array of angles between a sensor pair
pairs is an array of point pairs
radii is the guess at the radii of those angles
angles is an array of angles between a sensor pair pairs is an array of point pairs radii is the guess at the radii of those angles
static FLT calculateFitnessOld2(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
{
	FLT fitness = 0;
	for (size_t i = 0; i < numPairs; i++)
	{
		FLT v1[3], v2[3], diff[3];
		v1[0] = 1;
		v2[0] = 1;
		v1[1] = tan(angles[pairs[i].index1].HorizAngle); 
		v2[1] = tan(angles[pairs[i].index2].HorizAngle); 
		v1[2] = tan(angles[pairs[i].index1].VertAngle);  
		v2[2] = tan(angles[pairs[i].index2].VertAngle);  
		normalize3d(v1, v1); 
		normalize3d(v2, v2); 
		scale3d(v1, v1, radii[pairs[i].index1]);
		scale3d(v2, v2, radii[pairs[i].index2]);
		sub3d(diff, v1, v2);
		FLT distance = magnitude3d(diff);
		FLT t1 = magnitude3d(v1);
		FLT t2 = magnitude3d(v2);
		FLT estimatedDistanceBetweenPoints =
			SQUARED(radii[pairs[i].index1])
			+ SQUARED(radii[pairs[i].index2])
			- 2 * radii[pairs[i].index1] * radii[pairs[i].index2]
			* FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle)
			* FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle)
			+ FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle);
		fitness += SQUARED(distance - pairs[i].KnownDistance);
	}
	return FLT_SQRT(fitness);
}
static FLT calculateFitnessOld2 ( SensorAngles * angles , FLT * radii , PointPair * pairs , size_t numPairs ) { FLT fitness = 0 ; for ( size_t i = 0 ; i < numPairs ; i ++ ) { FLT v1 [ 3 ] , v2 [ 3 ] , diff [ 3 ] ; v1 [ 0 ] = 1 ; v2 [ 0 ] = 1 ; v1 [ 1 ] = tan ( angles [ pairs [ i ] . index1 ] . HorizAngle ) ; v2 [ 1 ] = tan ( angles [ pairs [ i ] . index2 ] . HorizAngle ) ; v1 [ 2 ] = tan ( angles [ pairs [ i ] . index1 ] . VertAngle ) ; v2 [ 2 ] = tan ( angles [ pairs [ i ] . index2 ] . VertAngle ) ; normalize3d ( v1 , v1 ) ; normalize3d ( v2 , v2 ) ; scale3d ( v1 , v1 , radii [ pairs [ i ] . index1 ] ) ; scale3d ( v2 , v2 , radii [ pairs [ i ] . index2 ] ) ; sub3d ( diff , v1 , v2 ) ; FLT distance = magnitude3d ( diff ) ; FLT t1 = magnitude3d ( v1 ) ; FLT t2 = magnitude3d ( v2 ) ; FLT estimatedDistanceBetweenPoints = SQUARED ( radii [ pairs [ i ] . index1 ] ) + SQUARED ( radii [ pairs [ i ] . index2 ] ) - 2 * radii [ pairs [ i ] . index1 ] * radii [ pairs [ i ] . index2 ] * FLT_SIN ( angles [ pairs [ i ] . index1 ] . HorizAngle ) * FLT_SIN ( angles [ pairs [ i ] . index2 ] . HorizAngle ) * FLT_COS ( angles [ pairs [ i ] . index1 ] . VertAngle - angles [ pairs [ i ] . index2 ] . VertAngle ) + FLT_COS ( angles [ pairs [ i ] . index1 ] . VertAngle ) * FLT_COS ( angles [ pairs [ i ] . index2 ] . VertAngle ) ; fitness += SQUARED ( distance - pairs [ i ] . KnownDistance ) ; } return FLT_SQRT ( fitness ) ; }
angles is an array of angles between a sensor pair
pairs is an array of point pairs
radii is the guess at the radii of those angles
angles is an array of angles between a sensor pair pairs is an array of point pairs radii is the guess at the radii of those angles
// These are the vectors that represent the direction for the two points.   // TODO: optimize by precomputing the tangent. // can be precomputed // can be precomputed // can be precomputed // can be precomputed // Now, normalize the vectors // can be precomputed // can be precomputed // Now, given the specified radii, find where the new points are // Cool, now find the vector between these two points // TODO: optimize the following two funcs into one. //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance);