// JlsForm01.cpp: implementation of the JlsForm01 class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "JlsForm01.h"




void	JlsForm01A( const double * x, const JVector * rl, const JVector * rw, JMatrix * A )
{
	//
	//	A = r + m * rl - rw
	//
	//	Parameters:
	//		x		(in)		x0, x1, x2, l0, l1, l2, l3		dim[7]
	//		rl		(in)		the point local to <x>
	//		rw		(in)		the point as global as <x>
	//		A		(out)		the differential vector			dim[3][1]
	//

	JRotation	m( x + 3 ) ;
	JVector		v( m * *rl ) ;

	(*A)[0][0]  =  x[0] + v.x - rw->x ;
	(*A)[1][0]  =  x[1] + v.y - rw->y ;
	(*A)[2][0]  =  x[2] + v.z - rw->z ;
}




void	JlsForm01Ax( const double * x, const JVector * rl, const JVector * rw, JMatrix * Ax )
{
	//
	//	A = r + m * rl - rw
	//
	//	Parameters:
	//		x		(in)		x0, x1, x2, l0, l1, l2, l3		dim[7]
	//		rl		(in)		the point local to <x>
	//		rw		(in)		the point as global as <x>
	//		Ax		(out)		the differential maxtrix		dim[3][7]
	//

	(*Ax)[0][0]  =  1 ;
	(*Ax)[0][1]  =  0 ;
	(*Ax)[0][2]  =  0 ;
	(*Ax)[0][3]  =  4 * x[3] * rl->x  -  2 * x[6] * rl->y  +  2 * x[5] * rl->z ;
	(*Ax)[0][4]  =  4 * x[4] * rl->x  +  2 * x[5] * rl->y  +  2 * x[6] * rl->z ;
	(*Ax)[0][5]  =                       2 * x[4] * rl->y  +  2 * x[3] * rl->z ;
	(*Ax)[0][6]  =                    -  2 * x[3] * rl->y  +  2 * x[4] * rl->z ;

	(*Ax)[1][0]  =  0 ;
	(*Ax)[1][1]  =  1 ;
	(*Ax)[1][2]  =  0 ;
	(*Ax)[1][3]  =  2 * x[6] * rl->x  +  4 * x[3] * rl->y  -  2 * x[4] * rl->z ;
	(*Ax)[1][4]  =  2 * x[5] * rl->x                       -  2 * x[3] * rl->z ;
	(*Ax)[1][5]  =  2 * x[4] * rl->x  +  4 * x[5] * rl->y  +  2 * x[6] * rl->z ;
	(*Ax)[1][6]  =  2 * x[3] * rl->x                       +  2 * x[5] * rl->z ;

	(*Ax)[2][0]  =  0 ;
	(*Ax)[2][1]  =  0 ;
	(*Ax)[2][2]  =  1 ;
	(*Ax)[2][3]  = -2 * x[5] * rl->x  +  2 * x[4] * rl->y  +  4 * x[3] * rl->z ;
	(*Ax)[2][4]  =  2 * x[6] * rl->x  +  2 * x[3] * rl->y                      ;
	(*Ax)[2][5]  = -2 * x[3] * rl->x  +  2 * x[6] * rl->y                      ;
	(*Ax)[2][6]  =  2 * x[4] * rl->x  +  2 * x[5] * rl->y  +  4 * x[6] * rl->z ;
}




void	JlsForm01Axjx( const double * x, const JVector * rl, const JVector * rw, JMatrix ** Axjx )
{
	//
	//	A = r + m * rl - rw
	//
	//	Parameters:
	//		x		(in)		x0, x1, x2, l0, l1, l2, l3		dim[7]
	//		rl		(in)		the point local to <x>
	//		rw		(in)		the point as global as <x>
	//		Axjx	(out)		the 7 differential maxtrix		dim[3][7] * 7
	//

	for ( int i=0 ;  i<7 ;  i++ )		Axjx[i]->Zero() ;

	(*Axjx[3])[0][3] = 4 * rl->x ;		(*Axjx[3])[0][5] = 2 * rl->z ;		(*Axjx[3])[0][6] =-2 * rl->y ;
	(*Axjx[3])[1][3] = 4 * rl->y ;		(*Axjx[3])[1][4] =-2 * rl->z ;		(*Axjx[3])[1][6] = 2 * rl->x ;
	(*Axjx[3])[2][3] = 4 * rl->z ;		(*Axjx[3])[2][4] = 2 * rl->y ;		(*Axjx[3])[2][5] =-2 * rl->x ;

	(*Axjx[4])[0][4] = 4 * rl->x ;		(*Axjx[4])[0][5] = 2 * rl->y ;		(*Axjx[4])[0][6] = 2 * rl->z ;
	(*Axjx[4])[1][3] =-2 * rl->z ;		(*Axjx[4])[1][5] = 2 * rl->x ;
	(*Axjx[4])[2][3] = 2 * rl->y ;		(*Axjx[4])[2][6] = 2 * rl->x ;

	(*Axjx[5])[0][3] = 2 * rl->z ;		(*Axjx[5])[0][4] = 2 * rl->y ;
	(*Axjx[5])[1][4] = 2 * rl->x ;		(*Axjx[5])[1][5] = 4 * rl->y ;		(*Axjx[5])[1][6] = 2 * rl->z ;
	(*Axjx[5])[2][3] =-2 * rl->x ;		(*Axjx[5])[2][6] = 2 * rl->y ;

	(*Axjx[6])[0][3] =-2 * rl->y ;		(*Axjx[6])[0][4] = 2 * rl->z ;
	(*Axjx[6])[1][3] = 2 * rl->x ;		(*Axjx[6])[1][5] = 2 * rl->z ;
	(*Axjx[6])[2][4] = 2 * rl->x ;		(*Axjx[6])[2][5] = 2 * rl->y ;		(*Axjx[6])[2][6] = 4 * rl->z ;
}