/*
 * $Id: vector.cpp,v 1.7 2002/07/14 21:40:43 msell Exp $
 *
 *
 * $Log: vector.cpp,v $
 * Revision 1.7  2002/07/14 21:40:43  msell
 * Conflictit pois, liikkumiset (hyppy, kävely, lyönti), uusi areena
 *
 * Revision 1.6  2002/07/04 21:05:41  msell
 * Se toimii!! =)
 * Törmäystarkistukset siis
 *
 * Revision 1.5  2002/06/27 00:08:04  msell
 * Kimmotukset palloille myös pyöritettyihin mesheihin
 *
 * Revision 1.4  2002/06/23 20:12:19  msell
 * Parempi törmäystarkistus palloista mesheihin
 *
 * Revision 1.3  2002/06/17 12:42:46  msell
 * Hieman parempi törmäysmallinnus taas
 *
 * Revision 1.2  2002/06/11 23:23:03  msell
 * Törmäystarkistusta
 *
 * Revision 1.1  2002/05/16 18:42:07  msell
 * Vektorifunctioita ja kamera
 *
 *
 *
 * $Date: 2002/07/14 21:40:43 $
 *
 */


#include <math.h>

#include "main.h"
#include "vector.h"
#include "utils.h"

void vectorSet(float *target, float x, float y, float z){
	target[0] = x;
	target[1] = y;
	target[2] = z;
}

void vectorCopy(float *target, float *source){
	target[0] = source[0];
	target[1] = source[1];
	target[2] = source[2];
}

void vectorAdd(float *target, float *source1, float *source2){
	target[0] = source1[0] + source2[0];
	target[1] = source1[1] + source2[1];
	target[2] = source1[2] + source2[2];
}

void vectorAdd(float *target, float *source){
	target[0] += source[0];
	target[1] += source[1];
	target[2] += source[2];
}

void vectorSub(float *target, float *source1, float *source2){
	target[0] = source1[0] - source2[0];
	target[1] = source1[1] - source2[1];
	target[2] = source1[2] - source2[2];
}

void vectorSub(float *target, float *source){
	target[0] -= source[0];
	target[1] -= source[1];
	target[2] -= source[2];
}

void vectorNegative(float *target, float *source){
	target[0] = -source[0];
	target[1] = -source[1];
	target[2] = -source[2];
}

void vectorNegative(float *target){
	target[0] = -target[0];
	target[1] = -target[1];
	target[2] = -target[2];
}

void vectorScale(float *target, float *source, float scale){
	target[0] = source[0] * scale;
	target[1] = source[1] * scale;
	target[2] = source[2] * scale;
}

void vectorScale(float *target, float scale){
	target[0] *= scale;
	target[1] *= scale;
	target[2] *= scale;
}

float vectorDot(float *source1, float *source2){
	return source1[0]*source2[0] + source1[1]*source2[1] + source1[2]*source2[2];
}

float vectorNormalizedDot(float *source1, float *source2){
	return vectorDot(source1, source2) / (vectorLength(source1) * vectorLength(source2));
}

float vectorLength(float *source){
	return sqrtf(vectorDot(source, source));
}

void vectorCross(float *target, float *source1, float *source2){
	target[0] = source1[1]*source2[2] - source1[2]*source2[1];
	target[1] = source1[2]*source2[0] - source1[0]*source2[2];
	target[2] = source1[0]*source2[1] - source1[1]*source2[0];
}

void vectorNormalize(float *target, float *source){
	vectorScale(target, source, 1.0/vectorLength(source));
}

void vectorNormalize(float *target){
	vectorScale(target, 1.0/vectorLength(target));
}

void vectorReflect(float *target, float *source, float *normal){
	vectorCopy(target, normal);
	vectorScale(target, -vectorDot(source, normal)*2);
	vectorAdd(target, source);
}

void vectorProject(float *target, float *source1, float *source2){
	vectorScale(target, source2, vectorDot(source1, source2) / vectorDot(source2, source2));
}

bool vectorIsZero(float *vector){
	if (vector[0] == 0 && vector[1] == 0 && vector[2] == 0) return true;
	//if (vectorDot(vector, vector) < 0.00001) return true;
	else return false;
}

void vectorSaturate(float *target, float *source, float min, float max){
	float len = vectorLength(source);
	if (len < min){
		len = min;
	} else if (len > max){
		len = max;
	} else{
		if (target != source) vectorCopy(target, source);
		return;
	}
	vectorNormalize(target, source);
	vectorScale(target, len);
}

void vectorSaturate(float *vector, float min, float max){
	vectorSaturate(vector, vector, min, max);
}

void vectorMatrixMultiply(float *target, float *source, float *matrix){
	float source2[3];
	if (source == target){
		vectorCopy(source2, source);
		source = source2;
	}
	int x;
	for (x = 0; x < 3; x++){
		target[x] = source[0]*matrix[0+x] + 
								source[1]*matrix[3+x] + 
								source[2]*matrix[6+x];
	}
}



void matrixSet(float *matrix,
							 float x1, float y1, float z1,
							 float x2, float y2, float z2,
							 float x3, float y3, float z3){
	matrix[0] = x1;
	matrix[1] = y1;
	matrix[2] = z1;
	matrix[3] = x2;
	matrix[4] = y2;
	matrix[5] = z2;
	matrix[6] = x3;
	matrix[7] = y3;
	matrix[8] = z3;
}

void matrixSet(float *matrix, float *r1, float *r2, float *r3){
	vectorCopy(&matrix[0], r1);
	vectorCopy(&matrix[3], r2);
	vectorCopy(&matrix[6], r3);
}

void matrixCopy(float *target, float *source){
	target[0] = source[0];
	target[1] = source[1];
	target[2] = source[2];
	target[3] = source[3];
	target[4] = source[4];
	target[5] = source[5];
	target[6] = source[6];
	target[7] = source[7];
	target[8] = source[8];
}

void matrixIdentity(float *matrix){
	matrixSet(matrix, 1, 0, 0, 0, 1, 0, 0, 0, 1);
}

void matrixAdd(float *target, float *source1, float *source2){
	target[0] = source1[0] + source2[0];
	target[1] = source1[1] + source2[1];
	target[2] = source1[2] + source2[2];
	target[3] = source1[3] + source2[3];
	target[4] = source1[4] + source2[4];
	target[5] = source1[5] + source2[5];
	target[6] = source1[6] + source2[6];
	target[7] = source1[7] + source2[7];
	target[8] = source1[8] + source2[8];
}

void matrixAdd(float *target, float *source){
	target[0] += source[0];
	target[1] += source[1];
	target[2] += source[2];
	target[3] += source[3];
	target[4] += source[4];
	target[5] += source[5];
	target[6] += source[6];
	target[7] += source[7];
	target[8] += source[8];
}

void matrixMultiply(float *target, float *source1, float *source2){
	float target2[9];
	float *oldtarget = target;
	bool copy = false;
	if (source1 == target || source2 == target){
		copy = true;
		target = target2;
	}
	int x, y;
	for (y = 0; y < 3; y++){
		for (x = 0; x < 3; x++){
			*target = source1[y*3+0]*source2[x] + 
								source1[y*3+1]*source2[3+x] + 
								source1[y*3+2]*source2[6+x];
			target++;
		}
	}
	if (copy){
		matrixCopy(oldtarget, target2);
	}
}

/*void matrixMultiply(float *target, float *source){
	matrixMultiply(target, source, source);
}*/

/*void matrixRotate(float *matrix, float *vector){
	float rotmat[9];
	createRotationMatrix(rotmat, vector);
	matrixMultiply(matrix, matrix, rotmat);
}*/

void matrixCreateRotation(float *matrix, float *vector){
	float angle = vectorLength(vector);
	float n[3];
	vectorNormalize(n, vector);

	float c = cos(angle);
	float s = sin(angle);
	float t = 1 - c;

	float x = n[0];
	float y = n[1];
	float z = n[2];

	matrixSet(matrix,
						t*x*x + c,   t*y*x + s*z, t*z*x - s*y,
						t*x*y - s*z, t*y*y + c,   t*z*y + s*x,
						t*x*z + s*y, t*y*z - s*x, t*z*z + c);

}

void matrixTranspose(float *target, float *source){
	target[0] = source[0]; target[1] = source[3]; target[2] = source[6];
	target[3] = source[1]; target[4] = source[4]; target[5] = source[7];
	target[6] = source[2]; target[7] = source[5]; target[8] = source[8];
}


/*void rotatePointAroundVector(float *target, float *point, float *vector){
	float angle = vectorLength(vector);
	float n[3];
	vectorNormalize(n, vector);

	float c = cos(angle);
	float s = sin(angle);
	float t = 1 - c;

	//r' = r*c + n*(n . r)*t + (r x n)*s
}*/