#ifndef lint
static char *sccsid = "@(#)quadric.c	1.2 (Steve Hill) 3/9/90";
#endif

/* quadric.c
 *
 * A general quadric equation can be represented as a matrix.
 *
 * | a b c d |
 * | b e f g |
 * | c f h i |
 * | d g i j |
 *
 * represents:
 *
 *   2     2     2
 * ax  + ey  + hz  + 2bxy + 2cxz + 2fyz + 2dx + 2gy + 2iz + j
 *
 */

#include <stdio.h>
#include <math.h>

#include "basetype.h"
#include "cartesian.h"
#include "error.h"
#include "matrix.h"
#include "point.h"
#include "vector.h"
#include "quadric.h"

/* QuadricCoef
 *
 * Gives matrix indices of coefficients of quadric equation.
 */

void
QuadricCoef(coef, i, j)
coef_t	coef;
int	*i, *j;
{
	switch (coef)
	{
	case QA:
		*i = *j = 0;
		break;
	case QB:
		*i = 0; *j = 1;
		break;
	case QC:
		*i = 0; *j = 2;
		break;
	case QD:
		*i = 0; *j = 3;
		break;
	case QE:
		*i = *j = 1;
		break;
	case QF:
		*i = 1; *j = 2;
		break;
	case QG:
		*i = 1; *j = 3;
		break;
	case QH:
		*i = *j = 2;
		break;
	case QI:
		*i = 2; *j = 3;
		break;
	case QJ:
		*i = *j = 3;
		break;
	default:
		FatalError(__FILE__, __LINE__, "QuadricCoef: Undefined coeficient");
	}
}


/* QuadricSet
 *
 * Sets a coefficient in a quadric equation.
 */

quadric_t *
QuadricSet(q, coef, v)
quadric_t	*q;
coef_t		coef;
real_t		v;
{
	int	i, j;
	
	QuadricCoef(coef, &i, &j);

	if (i == j)
		MatrixSet(QuadricMatrix(q), i, j, v);
	else
	{
		MatrixSet(QuadricMatrix(q), i, j, v);
		MatrixSet(QuadricMatrix(q), j, i, v);
	}
	return(q);
}


/* QuadricGet
 *
 * Retrieve a coefficient from a quadric equation.
 */

real_t
QuadricGet(q, coef)
quadric_t	*q;
coef_t		coef;
{
	int	i, j;

	QuadricCoef(coef, &i, &j);

	return(MatrixGet(q, i, j));
}


/* Quadric
 *
 * Construct a quadric  from its coefficients.
 */

quadric_t *
Quadric(a, b, c, d, e, f, g, h, i, j)
real_t	a, b, c, d, e, f, g, h, i, j;
{
	quadric_t	*q;

	q = (quadric_t *) Matrix(4, 4, (real_t *) NULL);

	QuadricSet(q, QA, a); QuadricSet(q, QB, b);
	QuadricSet(q, QC, c); QuadricSet(q, QD, d);
	QuadricSet(q, QE, e); QuadricSet(q, QF, f);
	QuadricSet(q, QG, g); QuadricSet(q, QH, h);
	QuadricSet(q, QI, i); QuadricSet(q, QJ, j);

	return((quadric_t *) q);
}


/* QuadricCoefs
 *
 * Bust a quadric into all its coefficients.
 */

void
QuadricCoefs(q, a, b, c, d, e, f, g, h, i, j)
quadric_t	*q;
real_t		*a, *b, *c, *d, *e, *f, *g, *h, *i, *j;
{
	*a = QuadricGet(q, QA);
	*b = QuadricGet(q, QB);
	*c = QuadricGet(q, QC);
	*d = QuadricGet(q, QD);
	*e = QuadricGet(q, QE);
	*f = QuadricGet(q, QF);
	*g = QuadricGet(q, QG);
	*h = QuadricGet(q, QH);
	*i = QuadricGet(q, QI);
	*j = QuadricGet(q, QJ);
}

/* QuadricZero
 *
 * Allocate and return a pointer to a quadric with zero coefficients
 */

quadric_t *
QuadricZero()
{
	matrix_t	*m;

	m = Matrix(4, 4, (real_t *) NULL);

	return((quadric_t *) m);
}

/* QuadricNorm
 *
 * Make small coefficients zero.
 */

quadric_t *
QuadricNorm(q)
quadric_t	*q;
{
	real_t	**m;
	int	r, c;

	m = q->matrix;

	for (r = 0; r < 4; r++)
	for (c = 0; c < 4; c++)
	{
		real_t	*v;

		v = &m[r][c];

		if (RealAbs(*v) < REAL_TINY)
			*v = REAL_ZERO;
	}

	return(q);
}


/* QuadricInstantiate
 *
 * Given x, y, z, instantiates the quadric and returns value.
 */

real_t
QuadricInstantiate(quad, x, y, z)
quadric_t	*quad;
real_t		x, y, z;
{
	real_t	a, b, c, d, e, f, g, h, i, j;

	QuadricCoefs(quad, &a, &b, &c, &d, &e, &f, &g, &h, &i, &j);

	/*   2     2     2                                            */
 	/* ax  + ey  + hz  + 2bxy + 2cxz + 2fyz + 2dx + 2gy + 2iz + j */
	/*                                                            */

	return((real_t)
               (a*x*x          + e*y*y          + h*z*z          +
	        REAL_TWO*b*x*y + REAL_TWO*c*x*z + REAL_TWO*f*y*z +
	        REAL_TWO*d*x   + REAL_TWO*g*y   + REAL_TWO*i*z   +
                j)
              );
}

/* QuadricNormal
 *
 * Given a point on the surface of a quadric, calculate the normal
 * vector at that point.
 *
 *  (2(ax + by + cz +d), 2(ey + bx + fz + g), 2(hz + cx + fy + i))
 */

vector_t *
QuadricNormal(vector, point, quadric)
vector_t	*vector;
point_t		*point;
quadric_t	*quadric;
{
	real_t	a, b, c, d, e, f, g, h, i, j;
	real_t	x, y, z;

	if (vector == VectorNull)
		vector = VectorZero();

	QuadricCoefs(quadric, &a, &b, &c, &d, &e, &f, &g, &h, &i, &j);
	PointGet(point, x, y, z);

	vector->x = a*x + b*y + c*z + d;	/* No need to multiply	*/
	vector->y = e*y + b*x + f*z + g;	/* by two since vector	*/
	vector->z = h*z + c*x + f*y + i;	/* is normalised	*/

	VectorNormalise(vector, vector);

	/* vector->x += 0.25 * sin(6.0 * x); */
	vector->z += 0.25 * sin(z);
	VectorNormalise(vector, vector);

	/*
	VectorPrint(stderr, vector);
	fprintf(stderr, "\n");
	*/

	return(vector);
}

/* QuadricNegate
 *
 * Negate all the coefficients (reverses normal).
 */

quadric_t *
QuadricNegate(quadric)
quadric_t	*quadric;
{
	(void) MatrixScale(QuadricMatrix(quadric),
			   QuadricMatrix(quadric),
			   -REAL_ONE);
	return(quadric);
}