src/poser_octavioradii.c
static FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
{
	FLT fitness = 0;
	for (size_t i = 0; i < numPairs; i++)
	{

		FLT angle = angleBetweenSensors(&angles[pairs[i].index1], &angles[pairs[i].index2]);

		// now we have a side-angle-side triangle, and we need to find the third side.

		// The Law of Cosines says: a^2 = b^2 + c^2 ? 2bc * cosA, 
		// where A is the angle opposite side a.

		// Transform this to:
		// a = sqrt(b^2 + c^2 - 2bc * cosA) and we know the length of the missing side!

		FLT b2 = (SQUARED(radii[pairs[i].index1]));
		FLT c2 = (SQUARED(radii[pairs[i].index2]));
		FLT bc2 = (2 * radii[pairs[i].index1] * radii[pairs[i].index2]);
		FLT cosA = (FLT_COS(angle));

		FLT angleInDegrees = angle * 180 / LINMATHPI;

		FLT dist = sqrt( (SQUARED(radii[pairs[i].index1])) + 
			             (SQUARED(radii[pairs[i].index2])) - 
			             (	(2 * radii[pairs[i].index1] * radii[pairs[i].index2]) *
							(FLT_COS(angle))));


		FLT fitnessAdder = SQUARED(dist - pairs[i].KnownDistance);

		if (isnan(fitnessAdder))
		{
			int a = 0;
		}

		//printf("  %2d %f\n", i, fitnessAdder);

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

	//fitness = 1 / fitness;
	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 calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
{
	FLT fitness = 0;
	for (size_t i = 0; i < numPairs; i++)
	{
		FLT angle = angleBetweenSensors(&angles[pairs[i].index1], &angles[pairs[i].index2]);
		FLT b2 = (SQUARED(radii[pairs[i].index1]));
		FLT c2 = (SQUARED(radii[pairs[i].index2]));
		FLT bc2 = (2 * radii[pairs[i].index1] * radii[pairs[i].index2]);
		FLT cosA = (FLT_COS(angle));
		FLT angleInDegrees = angle * 180 / LINMATHPI;
		FLT dist = sqrt( (SQUARED(radii[pairs[i].index1])) + 
			             (SQUARED(radii[pairs[i].index2])) - 
			             (	(2 * radii[pairs[i].index1] * radii[pairs[i].index2]) *
							(FLT_COS(angle))));
		FLT fitnessAdder = SQUARED(dist - pairs[i].KnownDistance);
		if (isnan(fitnessAdder))
		{
			int a = 0;
		}
		fitness += SQUARED(dist - pairs[i].KnownDistance);
	}
	return FLT_SQRT(fitness);
}
static FLT calculateFitness ( SensorAngles * angles , FLT * radii , PointPair * pairs , size_t numPairs ) { FLT fitness = 0 ; for ( size_t i = 0 ; i < numPairs ; i ++ ) { FLT angle = angleBetweenSensors ( & angles [ pairs [ i ] . index1 ] , & angles [ pairs [ i ] . index2 ] ) ; FLT b2 = ( SQUARED ( radii [ pairs [ i ] . index1 ] ) ) ; FLT c2 = ( SQUARED ( radii [ pairs [ i ] . index2 ] ) ) ; FLT bc2 = ( 2 * radii [ pairs [ i ] . index1 ] * radii [ pairs [ i ] . index2 ] ) ; FLT cosA = ( FLT_COS ( angle ) ) ; FLT angleInDegrees = angle * 180 / LINMATHPI ; FLT dist = sqrt ( ( SQUARED ( radii [ pairs [ i ] . index1 ] ) ) + ( SQUARED ( radii [ pairs [ i ] . index2 ] ) ) - ( ( 2 * radii [ pairs [ i ] . index1 ] * radii [ pairs [ i ] . index2 ] ) * ( FLT_COS ( angle ) ) ) ) ; FLT fitnessAdder = SQUARED ( dist - pairs [ i ] . KnownDistance ) ; if ( isnan ( fitnessAdder ) ) { int a = 0 ; } fitness += SQUARED ( dist - 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
// now we have a side-angle-side triangle, and we need to find the third side. // The Law of Cosines says: a^2 = b^2 + c^2 ? 2bc * cosA,  // where A is the angle opposite side a. // Transform this to: // a = sqrt(b^2 + c^2 - 2bc * cosA) and we know the length of the missing side! //printf("  %2d %f\n", i, fitnessAdder); //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); //fitness = 1 / fitness;