
#include "../precompiled.h"
#pragma hdrstop

idPluecker pluecker_origin( 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f );

/*
================
idPluecker::FromPlanes

  pluecker coordinate for the intersection of two planes
================
*/
bool idPluecker::FromPlanes( const idPlane &p1, const idPlane &p2 ) {

	p[0] = -( p1[2] * -p2[3] - p2[2] * -p1[3] );
	p[1] = -( p2[1] * -p1[3] - p1[1] * -p2[3] );
	p[2] = p1[1] * p2[2] - p2[1] * p1[2];

	p[3] = -( p1[0] * -p2[3] - p2[0] * -p1[3] );
	p[4] = p1[0] * p2[1] - p2[0] * p1[1];
	p[5] = p1[0] * p2[2] - p2[0] * p1[2];

	return ( p[2] != 0.0f || p[5] != 0.0f || p[4] != 0.0f );
}

/*
================
idPluecker::Distance3DSqr

  calculates square of shortest distance between the two
  3D lines represented by their pluecker coordinates
================
*/
float idPluecker::Distance3DSqr( const idPluecker &a ) const {
	float d, s;
	idVec3 dir;

	dir[0] = -a.p[5] *  p[4] -  a.p[4] * -p[5];
	dir[1] =  a.p[4] *  p[2] -  a.p[2] *  p[4];
	dir[2] =  a.p[2] * -p[5] - -a.p[5] *  p[2];
	if ( dir[0] == 0.0f && dir[1] == 0.0f && dir[2] == 0.0f ) {
		idVec3 start1, end1, start2, end2, offset, cross;
		const float epsilonSqr = VECTOR_EPSILON * VECTOR_EPSILON;

		if ( !ToLine( start1, end1 ) || !a.ToLine( start2, end2 ) ) {
			return -1.0f;
		}

		dir = end1 - start1;
		d = dir.LengthSqr();
		if ( d <= epsilonSqr ) {
			dir = end2 - start2;
			d = dir.LengthSqr();
			if ( d <= epsilonSqr ) {
				return -1.0f;
			}
		}

		offset = start2 - start1;
		cross = offset.Cross( dir );
		return cross.LengthSqr() / d;
	}
	d = a.p[4] * (  p[2] * dir[1] - -p[5] * dir[0] ) +
		a.p[5] * (  p[2] * dir[2] -  p[4] * dir[0] ) +
		a.p[2] * ( -p[5] * dir[2] -  p[4] * dir[1] );
	s = PermutedInnerProduct( a ) / d;
	return ( dir * dir ) * ( s * s );
}

/*
=============
idPluecker::ToString
=============
*/
const char *idPluecker::ToString( int precision ) const {
	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
}
