#include <libps.h>
#include "object.h"


void InitObject(ObjectHandler *object, u_long *pointer) {
	object->rot.vx = 0;
	object->rot.vy = 0;
	object->rot.vz = 0;
	GsInitCoordinate2(WORLD, &object->coord);
	object->handler.coord2 = &(object->coord);
	pointer++;
	GsMapModelingData(pointer);
	pointer += 2;
	GsLinkObject4((u_long )pointer, &object->handler, 0);
}


int Blend(int x, int y, int a, int b, int c, int d) {
	int t1, t2;
	
	x = x & 63;
	y = y & 63;
	t1 = a*64+x*(b-a);
	t2 = c*64+x*(d-c);
	return ((t1*64+y*(t2-t1))>>12);
}


/*void RotateModel(GsCOORDINATE2 *coord, SVECTOR *rot, int x, int y, int z) {
	MATRIX temp;

	ResetMatrix(coord->coord.m);
	rot->vx = (rot->vx+x) & 4095;
	rot->vy = (rot->vy+y) & 4095;
	rot->vz = (rot->vz+z) & 4095;
	RotMatrix(rot, &temp);
	MulMatrix0(&coord->coord, &temp, &coord->coord);
	coord->flg = 0;
}*/


void SetAcceleration(SVECTOR *vel, SVECTOR *rot, int d) {
	MATRIX matTmp;
	SVECTOR startVector;
	SVECTOR currentDirection;
	
	if (d != 0) {
		startVector.vx = 0;
		startVector.vy = 0;
		startVector.vz = ONE;
		RotMatrix(rot, &matTmp);
		ApplyMatrixSV(&matTmp, &startVector, &currentDirection);

		vel->vx += (currentDirection.vx * d)>>12;
//		vel->vy += (currentDirection.vy * d)>>12;
		vel->vz += (currentDirection.vz * d)>>12;
	}
}


void ResetMatrix(short m[3][3]) {
	m[0][0] = m[1][1] = m[2][2] = ONE;
	m[0][1] = m[0][2] = m[1][0] = 0;
	m[1][2] = m[2][0] = m[2][1] = 0;
}