// ### OpenGTA3 ###
// math primitives (matrix, sphere, rect, quaternion, ...)

#include "stdafx.h"
///#include "gtaInterface.h"
#include "ogMath.h"

// TODO: заглушки-времянки
extern int (*RwMatrixDestroy)(RwMatrix *);

//
int random (int l, int h) {
	return (int)((float)rand()/32768.0f*(float)(h-l))+l;
}
//=============================================================================
void RwMatrix::makeIdentityMatrix(float value) {
	memset (this, 0, sizeof (RwMatrix));
	right.x = top.y = at.z = value;
	flags = 0x2003;
}
//=============================================================================
// vector operations
float dotProduct (D3DVECTOR * v1, D3DVECTOR * v2) {
	return v1->x*v2->x+v1->y*v2->y+v1->z*v2->z;
}

D3DVECTOR * crossProduct (D3DVECTOR * out, D3DVECTOR * a, D3DVECTOR * b) {
	out->x = b->z*a->y - b->y*a->z;
	out->y = b->x*a->z - b->z*a->x;
	out->z = b->y*a->x - b->x*a->y;
	return out;
}

D3DVECTOR * vectorAdd (D3DVECTOR * out, D3DVECTOR * in1, D3DVECTOR * in2) {
	out->x = in1->x+in2->x;
	out->y = in1->y+in2->y;
	out->z = in1->z+in2->z;
	return out;
}

D3DVECTOR * vectorAddF (D3DVECTOR * out, D3DVECTOR * in1, float in2) {
	out->x = in1->x + in2;
	out->y = in1->y + in2;
	out->z = in1->z + in2;
	return out;
}

D3DVECTOR * vectorSub (D3DVECTOR * out, D3DVECTOR * in1, D3DVECTOR * in2) {
	out->x = in1->x-in2->x;
	out->y = in1->y-in2->y;
	out->z = in1->z-in2->z;
	return out;
}

D3DVECTOR * vectorSubF (D3DVECTOR * out, D3DVECTOR * in1, float in2) {
	out->x = in1->x - in2;
	out->y = in1->y - in2;
	out->z = in1->z - in2;
	return out;
}

//=============================================================================
// sphere
void RwSphere::init (float _radius, D3DVECTOR * _center) {	// >> 40FCF0 (25)
	radius = _radius; 
	center = *_center; 
}
//=============================================================================
// matrix operations
RwMatrix * RwMatrixUpdate (RwMatrix * m) {	
	m->flags &= 0xFFFDFFFC;
	return m;
}

//=============================================================================
// CMatrix
CMatrix::~CMatrix() {
	if (ownMatrix && rwm)
		RwMatrixDestroy (rwm);
}

void CMatrix::setIdentityMatrix () {
	m.right.x = m.top.y = m.at.z = 1.0f;
	m.right.y = m.right.z = m.top.x = m.top.z = m.at.x = m.at.y = m.pos.x = m.pos.y = m.pos.z = 0.0f;
}

RwMatrix * CMatrix::copyTo(RwMatrix *target) {
	target->right = m.right;
	target->top = m.top;
	target->at = m.at;
	target->pos = m.pos;
	return RwMatrixUpdate (&m);
}

void CMatrix::fullCopyFrom (RwMatrix * src) {
	m = *src;
}

void CMatrix::setPos (float x, float y , float z) {
	m.pos.x = x;
	m.pos.y = y;
	m.pos.z = z;
}

void CMatrix::attach(RwMatrix *matrix, BYTE own) {
	if (rwm && ownMatrix) 
		RwMatrixDestroy (rwm);
	rwm = matrix;
	ownMatrix = own;
	m = *matrix;	// NOTE: изначально, флаги не копировались
}

// dst = m * point 
void transformPoint (D3DVECTOR * dst, CMatrix * m, D3DVECTOR * point) {
	dst->x = m->m.at.x*point->z + m->m.top.x*point->y + m->m.right.x*point->x + m->m.pos.x;
	dst->y = m->m.at.y*point->z + m->m.top.y*point->y + m->m.right.y*point->x + m->m.pos.y;
	dst->z = m->m.at.z*point->z + m->m.top.z*point->y + m->m.right.z*point->x + m->m.pos.z;
}
//=============================================================================
// CQuaternion
void CQuaternion::get (RwMatrix * m) {
	float T = m->top.y + m->right.x + m->at.z;
	float S;
	if (T > 0.0f) {
		S = sqrt (1.0f+T)*2.0f;
	
		imag.x = (m->top.z - m->at.y)/S;
		imag.y = (m->at.x - m->right.z)/S;
		imag.z = (m->right.y - m->top.x)/S;
		real = S/4.0f;
		return;
	} 
	T = m->right.x - m->top.y - m->at.z;
	if (T > 0.0f) {
		S = sqrt (1.0f+T)*2.0f;

		imag.x = S/4.0f;
		imag.y = (m->top.x + m->right.y)/S;
		imag.z = (m->at.x + m->right.z)/S;
		real = (m->top.z - m->at.y)/S;
		return;
	}
	T = m->top.y - m->right.x - m->at.z;
	if (T > 0.0f) {
		S = sqrt (1.0f+T)*2.0f;

		imag.x = (m->top.x - m->right.y)/S;
		imag.y = S/4.0f;
		imag.z = (m->at.y + m->top.z)/S;
		real = (m->at.x - m->right.z)/S;
		return;
	}
	T = m->at.z - m->top.y - m->right.x;
	S = sqrt (1.0f+T)*2.0f;

	imag.x = (m->at.x + m->right.z)/S;
	imag.y = (m->at.y + m->top.z)/S;
	imag.z = S/4.0f;
	real  = (m->right.y - m->top.x)/S;
}
//=============================================================================
// CRect
void CRect::includePoint(float x, float y) {
	if (x < l)
		l = x;
	if (x > r)
		r = x;
	if (y < b) 
		b = y;
	if (y > t)
		t = y;
};
//=============================================================================
// RwBBox
RwBBox::RwBBox(RwSphere *sphere) {	// create bounding box from bounding sphere
	vectorSubF (&inf, &(sphere->center), sphere->radius);
	vectorAddF (&sup, &(sphere->center), sphere->radius);
}
//=============================================================================
