#include "penddefs.h"

#define EPS		1e-4

void
sduperr( double t, double q[], double err[] )
{
	int i;
	double gp[3];

	for( i=0; i<NUM_STATE; i++ )
	{
		sdpos( i, p1, gp );
		if ( gp[1] < GROUND_HEIGHT )
			err[i] = 0.5 * ( GROUND_HEIGHT - gp[1] ) * ( GROUND_HEIGHT - gp[1] );
		else
			err[i] = 0.0;
	}
}

void
sduverr( double t, double q[], double u[], double err[] )
{
	int i;
	double gp[3];
	double gv[3];

	for( i=0; i<NUM_STATE; i++ )
	{
		sdpos( i, p1, gp );
		sdvel( i, p1, gv );
		if ( gp[1] < GROUND_HEIGHT )
			err[i] = ( - gv[1] ) * ( GROUND_HEIGHT - gp[1] );
		else
			err[i] = 0.0;
	}
}

void
sduaerr( double t, double q[], double u[], double udot[], double err[] )
{
	int i;
	double gp[3];
	double gv[3];
	double ga[3];

	for( i=0; i<NUM_STATE; i++ )
	{
		sdpos( i, p1, gp );
		sdvel( i, p1, gv );
		sdacc( i, p1, ga );
		if ( gp[1] < GROUND_HEIGHT )
			err[i] = ( - ga[1] ) * ( GROUND_HEIGHT - gp[1] ) + ( gv[1] * gv[1] );
		else
			err[i] = 0.0;
	}
}

void
sduconsfrc( double t, double q[], double u[], double mults[] )
{
	int i;
	double gp[3];
	double frc[3];

	for( i=0; i<NUM_STATE; i++ )
	{
		frc[0] = frc[1] = frc[2] = 0;

		sdpos( i, p1, gp );
		frc[1] = -mults[i] * ( GROUND_HEIGHT - gp[1] );

		sdtrans( -1, frc, i, frc );
		sdpointf( i, p1, frc );
	}
}