/*  Acknex-style float vectors

free for any use

created by 'Kartoffel', last edited on: 31.01.2017

// description

floating-point vectors, similar to VECTOR/ANGLE in acknex.
most (but not all) standard vec_-functions are included.
there are also some additional functions.

note: - for more info read the following lines (up to line ~100)
      - some minor settings at line ~100
      - not all functions have been tested thoroughly
      - vec_bounce works differently here! (acknex's vec_bounce buggy?)
      - ang_rotate rotates 'roll' differently here

*/

#ifndef _FVEC_H_
#define _FVEC_H_

//// Datatypes

// FVECTOR: like VECTOR; floating point vector type, contains: x, y, z
typedef struct FVECTOR
{
	float x, y, z;
} FVECTOR;

// FANGLE: like ANGLE; floating point angle type, contains: pan, tilt, roll (FVECTOR can be used instead, aswell; (same as VECTOR/ANGLE compatibility))
typedef struct FANGLE
{
	float pan, tilt, roll;
} FANGLE;


// startup function, is automatically called!
void _FVEC_startp();

// non-zero when startup function is done
var FVEC_INITIALIZED = 0;

// FVECTOR version of 'nullvector' (using floats)
FVECTOR * fnullvector = NULL;

// returns temporary FVECTOR* with given XYZ (like vector() for VECTOR), buffer size configurable at settings section
FVECTOR * fvector(float X, float Y, float Z);


//// VECTOR -> FVECTOR conversion

// copies VECTOR* pVecB to FVECTOR* pVecA
void fvec_from_vec(FVECTOR * pVecA, VECTOR * pVecB);

// returns VECTOR* pVecB as temporary FVECTOR* (using fvector()-function)
FVECTOR * fvec_from_vec(VECTOR * pVecB);


//// FVECTOR -> VECTOR conversion

// copies FVECTOR* pVecB to VECTOR* pVecA
void * vec_from_fvec(VECTOR * pVecA, FVECTOR * pVecB);

// returns FVECTOR* pVecB as temporary VECTOR* (using vector()-function)
VECTOR * fvec_from_vec(VECTOR * pVecB);


//// FVECTOR functions (work the same as Acknex's VECTOR/ANGLE-functions, but use FVECTOR/FANGLE/float instead of VECTOR/ANGLE/var)

void fvec_set(FVECTOR * pVecA, FVECTOR * pVecB);
void fvec_set_xyz(FVECTOR * pVec, float X, float Y, float Z);
void fvec_fill(FVECTOR * pVec, float Value);
void fvec_add(FVECTOR * pVecA, FVECTOR * pVecB);
void fvec_sub(FVECTOR * pVecA, FVECTOR * pVecB);
void fvec_scale(FVECTOR * pVec, float Factor);
void fvec_inverse(FVECTOR * pVec);
float fvec_length(FVECTOR * pVec);
float fvec_dist(FVECTOR * pVecA, FVECTOR * pVecB);
void fvec_normalize(FVECTOR * pVec, float Length);
void fvec_normalize(FVECTOR * pVec); // uses length = 1
float fvec_dot(FVECTOR * pVecA, FVECTOR * pVecB);
float fvec_dot_norm(FVECTOR * pVecA, FVECTOR * pVecB); // uses normalized vectors
void fvec_cross(FVECTOR * pVecTarget, FVECTOR * pVecA, FVECTOR * pVecB);
void fvec_bounce(FVECTOR * pVecRay, FVECTOR * pVecNormal);
void fvec_bounce_norm(FVECTOR * pVecRay, FVECTOR * pVecNormal); // uses normalized normal vector
void fvec_lerp(FVECTOR * pVecTarget, FVECTOR * pVecA, FVECTOR * pVecB, float BlendFactor);
void fvec_slerp(FVECTOR * pVecTarget, FVECTOR * pVecA, FVECTOR * pVecB, float BlendFactor); // same as lerp but using sine-falloff
void fvec_for_angle(FVECTOR * pVec, FANGLE * pAng); // uses degrees
void fvec_to_angle(FANGLE * pAng, FVECTOR * pVec); // uses degrees
void fvec_rotate(FVECTOR * pVec, FANGLE * pAng); // uses degrees
void fang_rotate(FANGLE * pAngA, FANGLE * pAngB); // uses degrees
void fvec_for_angle_rad(FVECTOR * pVec, FANGLE * pAng); // uses radians
void fvec_to_angle_rad(FANGLE * pAng, FVECTOR * pVec); // uses radians
void fvec_rotate_rad(FVECTOR * pVec, FANGLE * pAng); // uses radians
void fang_rotate_rad(FANGLE * pAngA, FANGLE * pAngB); // uses radians
float fvec_accelerate(FVECTOR * pVecSpeed, FVECTOR * pVecAccel, float Friction);

//// Settings:

//#define FVEC_UNSAFE // disables pointer checking

#define FVEC_USE_TEMP_VECTORS // silghtly slower, allows to use one vector as both input and output (for example: fvec_cross(VecA, VecA, VecB))

#define FVEC_FVECTOR_BUFFER_NUM 32 // number of temporary vectors for the fvector() function

/// no more info / settings from here on
//////////////////////////////////////////////////

#ifndef DEG_TO_RAD
	#define DEG_TO_RAD(X) ((X) * 3.141593 / 180.0)
#endif

#ifndef RAD_TO_DEG
	#define RAD_TO_DEG(X) ((X) * 180.0 / 3.141593)
#endif

//

int __fvector_buffer_current = 0;
FVECTOR __fvector_buffer[FVEC_FVECTOR_BUFFER_NUM];

//

FVECTOR * fvector(float X, float Y, float Z)
{
	int this_fvec = __fvector_buffer_current;
	__fvector_buffer_current = (__fvector_buffer_current + 1) % FVEC_FVECTOR_BUFFER_NUM;
	
	__fvector_buffer[this_fvec].x = X;
	__fvector_buffer[this_fvec].y = Y;
	__fvector_buffer[this_fvec].z = Z;
	
	return &(__fvector_buffer[this_fvec]);
}

void fvec_from_vec(FVECTOR * pVecA, VECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_from_vec()':nnempty pointer.");
			return;
		}
	#endif
	
	pVecA->x = (float)pVecB->x;
	pVecA->y = (float)pVecB->y;
	pVecA->z = (float)pVecB->z;
}

void vec_from_fvec(VECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'vec_from_fvec()':nnempty pointer.");
			return;
		}
	#endif
	
	pVecA->x = (var)pVecB->x;
	pVecA->y = (var)pVecB->y;
	pVecA->z = (var)pVecB->z;
}

FVECTOR * fvec_from_vec(VECTOR * pVec)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'fvec_from_vec()':nnempty pointer.");
			return fnullvector;
		}
	#endif
	
	return fvector((float)pVec->x, (float)pVec->y, (float)pVec->z);
}

VECTOR * vec_from_fvec(FVECTOR * pVec)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'vec_from_fvec()':nnempty pointer.");
			return nullvector;
		}
	#endif
	
	return vector((var)pVec->x, (var)pVec->y, (var)pVec->z);
}

void fvec_set(FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_set()':nnempty pointer.");
			return;
		}
	#endif
	
	memcpy(pVecA, pVecB, sizeof(float) * 3);
}

void fvec_set_xyz(FVECTOR * pVec, float X, float Y, float Z)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'fvec_set_xyz()':nnempty pointer.");
			return;
		}
	#endif
	
	memcpy(pVec, &X, sizeof(float) * 3);
}

void fvec_fill(FVECTOR * pVec, float Value)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'fvec_fill()':nnempty pointer.");
			return;
		}
	#endif
	
	pVec->x = pVec->y = pVec->z = Value;
}

void fvec_add(FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_add()':nnempty pointer.");
			return;
		}
	#endif
	
	pVecA->x += pVecB->x;
	pVecA->y += pVecB->y;
	pVecA->z += pVecB->z;
}

void fvec_sub(FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_sub()':nnempty pointer.");
			return;
		}
	#endif
	
	pVecA->x -= pVecB->x;
	pVecA->y -= pVecB->y;
	pVecA->z -= pVecB->z;
}

void fvec_diff(FVECTOR * pVecTarget, FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecTarget == NULL || pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_diff()':nnempty pointer.");
			return;
		}
	#endif
	
	#ifdef FVEC_USE_TEMP_VECTORS
		FVECTOR TempVec;
		
		fvec_set(&TempVec, pVecA);
		fvec_sub(&TempVec, pVecB);
		fvec_set(pVecTarget, &TempVec);
	#else
		fvec_set(pVecTarget, pVecA);
		fvec_sub(pVecTarget, pVecB);
	#endif
}

void fvec_scale(FVECTOR * pVec, float Factor)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'fvec_scale()':nnempty pointer.");
			return;
		}
	#endif
	
	pVec->x *= Factor;
	pVec->y *= Factor;
	pVec->z *= Factor;
}

void fvec_mul(FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_mul()':nnempty pointer.");
			return;
		}
	#endif
	
	pVecA->x *= pVecB->x;
	pVecA->y *= pVecB->y;
	pVecA->z *= pVecB->z;
}

void fvec_inverse(FVECTOR * pVec)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'fvec_inverse()':nnempty pointer.");
			return;
		}
	#endif
	
	pVec->x = -pVec->x;
	pVec->y = -pVec->y;
	pVec->z = -pVec->z;
}

float fvec_length(FVECTOR * pVec)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'fvec_length()':nnempty pointer.");
			return 0;
		}
	#endif
	
	return sqrt(pVec->x * pVec->x + pVec->y * pVec->y + pVec->z * pVec->z);
}

float fvec_dist(FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_dist()':nnempty pointer.");
			return 0;
		}
	#endif
	
	FVECTOR TempVec;
	
	fvec_set(&TempVec, pVecB);
	fvec_sub(&TempVec, pVecA);
	
	return fvec_length(&TempVec);
}

void fvec_normalize(FVECTOR * pVec, float Length)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'fvec_normalize()':nnempty pointer.");
			return;
		}
	#endif
	
	float CurrentLength = fvec_length(pVec);
	
	if(CurrentLength != 0.0)
	{
		fvec_scale(pVec, Length / CurrentLength);
	}
}

void fvec_normalize(FVECTOR * pVec)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL)
		{
			printf("error in 'fvec_normalize()':nnempty pointer.");
			return;
		}
	#endif
	
	float CurrentLength = fvec_length(pVec);
	
	if(CurrentLength != 0.0)
	{
		fvec_scale(pVec, 1.0 / CurrentLength);
	}
}

float fvec_dot(FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_dot()':nnempty pointer.");
			return 0;
		}
	#endif
	
	return pVecA->x * pVecB->x + pVecA->y * pVecB->y + pVecA->z * pVecB->z;
}

float fvec_dot_norm(FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_dot_norm()':nnempty pointer.");
			return 0;
		}
	#endif
	
	float Dot = pVecA->x * pVecB->x + pVecA->y * pVecB->y + pVecA->z * pVecB->z;
	
	float Length = fvec_length(pVecA) * fvec_length(pVecB);
	
	if(Length != 0.0)
	{
		Dot /= Length;
	}
	
	return Dot;
}

void fvec_cross(FVECTOR * pVecTarget, FVECTOR * pVecA, FVECTOR * pVecB)
{
	#ifndef FVEC_UNSAFE
		if(pVecTarget == NULL || pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_cross()':nnempty pointer.");
			return;
		}
	#endif
	
	#ifdef FVEC_USE_TEMP_VECTORS
		FVECTOR TempVec;
		
		TempVec.x = pVecA->y * pVecB->z - pVecA->z * pVecB->y;
		TempVec.y = pVecA->z * pVecB->x - pVecA->x * pVecB->z;
		TempVec.z = pVecA->x * pVecB->y - pVecA->y * pVecB->x;
		
		fvec_set(pVecTarget, &TempVec);
	#else
		pVecTarget->x = pVecA->y * pVecB->z - pVecA->z * pVecB->y;
		pVecTarget->y = pVecA->z * pVecB->x - pVecA->x * pVecB->z;
		pVecTarget->z = pVecA->x * pVecB->y - pVecA->y * pVecB->x;
	#endif
}

void fvec_bounce(FVECTOR * pVecRay, FVECTOR * pVecNormal)
{
	#ifndef FVEC_UNSAFE
		if(pVecRay == NULL || pVecNormal == NULL)
		{
			printf("error in 'fvec_bounce()':nnempty pointer.");
			return;
		}
	#endif
	
	float Dot = fvec_dot(pVecNormal, pVecRay);

	pVecRay->x -= 2.0 * Dot * pVecNormal->x;
	pVecRay->y -= 2.0 * Dot * pVecNormal->y;
	pVecRay->z -= 2.0 * Dot * pVecNormal->z;
}

void fvec_bounce_norm(FVECTOR * pVecRay, FVECTOR * pVecNormal)
{
	#ifndef FVEC_UNSAFE
		if(pVecRay == NULL || pVecNormal == NULL)
		{
			printf("error in 'fvec_bounce_norm()':nnempty pointer.");
			return;
		}
	#endif
	
	FVECTOR Normal;
	fvec_set(&Normal, pVecNormal);
	fvec_normalize(&Normal);
	
	float Dot = fvec_dot(&Normal, pVecRay);

	pVecRay->x -= 2.0 * Dot * Normal.x;
	pVecRay->y -= 2.0 * Dot * Normal.y;
	pVecRay->z -= 2.0 * Dot * Normal.z;
}

// lerp
void fvec_lerp(FVECTOR * pVecTarget, FVECTOR * pVecA, FVECTOR * pVecB, float BlendFactor)
{
	#ifndef FVEC_UNSAFE
		if(pVecTarget == NULL || pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_lerp()':nnempty pointer.");
			return;
		}
	#endif
	
	#ifdef FVEC_USE_TEMP_VECTORS
		FVECTOR TempVec;
		
		TempVec.x = pVecA->x + (pVecB->x - pVecA->x) * BlendFactor;
		TempVec.y = pVecA->y + (pVecB->y - pVecA->y) * BlendFactor;
		TempVec.z = pVecA->z + (pVecB->z - pVecA->z) * BlendFactor;
		
		fvec_set(pVecTarget, &TempVec);
	#else
		pVecTarget->x = pVecA->x + (pVecB->x - pVecA->x) * BlendFactor;
		pVecTarget->y = pVecA->y + (pVecB->y - pVecA->y) * BlendFactor;
		pVecTarget->z = pVecA->z + (pVecB->z - pVecA->z) * BlendFactor;
	#endif
}

// smooth lerp (sine)
void fvec_slerp(FVECTOR * pVecTarget, FVECTOR * pVecA, FVECTOR * pVecB, float BlendFactor)
{
	#ifndef FVEC_UNSAFE
		if(pVecTarget == NULL || pVecA == NULL || pVecB == NULL)
		{
			printf("error in 'fvec_slerp()':nnempty pointer.");
			return;
		}
	#endif
	
	BlendFactor = clamp(BlendFactor, 0.0, 1.0);
	BlendFactor = BlendFactor * BlendFactor * (3.0 - BlendFactor * 2.0);
	
	#ifdef FVEC_USE_TEMP_VECTORS
		FVECTOR TempVec;
		
		TempVec.x = pVecA->x + (pVecB->x - pVecA->x) * BlendFactor;
		TempVec.y = pVecA->y + (pVecB->y - pVecA->y) * BlendFactor;
		TempVec.z = pVecA->z + (pVecB->z - pVecA->z) * BlendFactor;
		
		fvec_set(pVecTarget, &TempVec);
	#else
		pVecTarget->x = pVecA->x + (pVecB->x - pVecA->x) * BlendFactor;
		pVecTarget->y = pVecA->y + (pVecB->y - pVecA->y) * BlendFactor;
		pVecTarget->z = pVecA->z + (pVecB->z - pVecA->z) * BlendFactor;
	#endif
}

// uses degrees
void fvec_for_angle(FVECTOR * pVec, FANGLE * pAng)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL || pAng == NULL)
		{
			printf("error in 'fvec_for_angle()':nnempty pointer.");
			return;
		}
	#endif
	
	float pan = DEG_TO_RAD(pAng->pan);
	float tilt = DEG_TO_RAD(pAng->tilt);
	float roll = DEG_TO_RAD(pAng->roll);
	
	pVec->x = cos(pan) * cos(tilt);
	pVec->y = sin(pan) * cos(tilt);
	pVec->z = sin(tilt);
}

// uses radians
void fvec_for_angle_rad(FVECTOR * pVec, FANGLE * pAng)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL || pAng == NULL)
		{
			printf("error in 'fvec_for_angle_rad()':nnempty pointer.");
			return;
		}
	#endif
	
	pVec->x = cos(pAng->pan) * cos(pAng->tilt);
	pVec->y = sin(pAng->pan) * cos(pAng->tilt);
	pVec->z = sin(pAng->tilt);
}

// uses degrees
void fvec_to_angle(FANGLE * pAng, FVECTOR * pVec)
{
	#ifndef FVEC_UNSAFE
		if(pAng == NULL || pVec == NULL)
		{
			printf("error in 'fvec_to_angle()':nnempty pointer.");
			return;
		}
	#endif
	
	pAng->pan = RAD_TO_DEG(atan(pVec->y / pVec->x));
	pAng->tilt = RAD_TO_DEG(asin(pVec->z / fvec_length(pVec)));
	pAng->roll = 0.0;
}

// uses radians
void fvec_to_angle_rad(FANGLE * pAng, FVECTOR * pVec)
{
	#ifndef FVEC_UNSAFE
		if(pAng == NULL || pVec == NULL)
		{
			printf("error in 'fvec_to_angle_rad()':nnempty pointer.");
			return;
		}
	#endif
	
	pAng->pan = atan(pVec->y / pVec->x);
	pAng->tilt = asin(pVec->z / fvec_length(pVec));
	pAng->roll = 0.0;
}

// uses degrees
void fvec_rotate(FVECTOR * pVec, FANGLE * pAng)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL || pAng == NULL)
		{
			printf("error in 'fvec_rotate()':nnempty pointer.");
			return;
		}
	#endif
	
	float pan_sin = sin(DEG_TO_RAD(pAng->pan));
	float pan_cos = cos(DEG_TO_RAD(pAng->pan));
	float tilt_sin = sin(DEG_TO_RAD(pAng->tilt));
	float tilt_cos = cos(DEG_TO_RAD(pAng->tilt));
	float roll_sin = sin(DEG_TO_RAD(pAng->roll));
	float roll_cos = cos(DEG_TO_RAD(pAng->roll));
	
	FVECTOR new;
	
	new.x = pVec->x;
	new.y = pVec->y * roll_cos - pVec->z * roll_sin;
	new.z = pVec->y * roll_sin + pVec->z * roll_cos;
	
	pVec->x = new.x * tilt_cos - new.z * tilt_sin;
	pVec->y = new.y;
	pVec->z = new.x * tilt_sin + new.z * tilt_cos;
	
	new.x = pVec->x * pan_cos - pVec->y * pan_sin;
	new.y = pVec->x * pan_sin + pVec->y * pan_cos;
	new.z = pVec->z;
	
	fvec_set(pVec, &new);
}

// uses radians
void fvec_rotate_rad(FVECTOR * pVec, FANGLE * pAng)
{
	#ifndef FVEC_UNSAFE
		if(pVec == NULL || pAng == NULL)
		{
			printf("error in 'fvec_rotate_rad()':nnempty pointer.");
			return;
		}
	#endif
	
	float pan_sin = sin(pAng->pan);
	float pan_cos = cos(pAng->pan);
	float tilt_sin = sin(pAng->tilt);
	float tilt_cos = cos(pAng->tilt);
	float roll_sin = sin(pAng->roll);
	float roll_cos = cos(pAng->roll);
	
	FVECTOR new;
	
	new.x = pVec->x;
	new.y = pVec->y * roll_cos - pVec->z * roll_sin;
	new.z = pVec->y * roll_sin + pVec->z * roll_cos;
	
	pVec->x = new.x * tilt_cos - new.z * tilt_sin;
	pVec->y = new.y;
	pVec->z = new.x * tilt_sin + new.z * tilt_cos;
	
	new.x = pVec->x * pan_cos - pVec->y * pan_sin;
	new.y = pVec->x * pan_sin + pVec->y * pan_cos;
	new.z = pVec->z;
	
	fvec_set(pVec, &new);
}

void fang_rotate(FANGLE * pAngA, FANGLE * pAngB)
{
	#ifndef FVEC_UNSAFE
		if(pAngA == NULL || pAngB == NULL)
		{
			printf("error in 'fang_rotate()':nnempty pointer.");
			return;
		}
	#endif
	
	FVECTOR TempVec;
	
	float roll = pAngA->roll + pAngB->roll;
	
	fvec_for_angle(&TempVec, pAngB);
	fvec_rotate(&TempVec, pAngA);
	fvec_to_angle(pAngA, &TempVec);
	
	pAngA->roll = roll;
}

void fang_rotate_rad(FANGLE * pAngA, FANGLE * pAngB)
{
	#ifndef FVEC_UNSAFE
		if(pAngA == NULL || pAngB == NULL)
		{
			printf("error in 'fang_rotate_rad()':nnempty pointer.");
			return;
		}
	#endif
	
	FVECTOR TempVec;
	
	float roll = pAngA->roll + pAngB->roll;
	
	fvec_for_angle_rad(&TempVec, pAngA);
	fvec_rotate_rad(&TempVec, pAngB);
	fvec_to_angle_rad(pAngA, &TempVec);
	
	pAngA->roll = roll;
}

float fvec_accelerate(FVECTOR * pVecSpeed, FVECTOR * pVecAccel, float Friction)
{
	#ifndef FVEC_UNSAFE
		if(pVecSpeed == NULL || pVecAccel == NULL)
		{
			printf("error in 'fvec_accelerate()':nnempty pointer.");
			return;
		}
	#endif
	
	FVECTOR Distance;
	
	float ftime_step = (float)time_step;
	
	if(Friction == 0)
	{
		Distance.x = pVecSpeed->x * ftime_step + pVecAccel->x * ftime_step * ftime_step * 0.5;
		Distance.y = pVecSpeed->y * ftime_step + pVecAccel->y * ftime_step * ftime_step * 0.5;
		Distance.z = pVecSpeed->z * ftime_step + pVecAccel->z * ftime_step * ftime_step * 0.5;
		
		pVecSpeed->x += pVecAccel->x * ftime_step;
		pVecSpeed->y += pVecAccel->y * ftime_step;
		pVecSpeed->z += pVecAccel->z * ftime_step;
	}
	else
	{
		float InvFriction = 1.0 / Friction;
		float TempFactor = (1.0 - exp(-Friction * ftime_step)) * InvFriction;
		
		Distance.x = pVecAccel->x * ftime_step * InvFriction + (pVecSpeed->x - pVecAccel->x * InvFriction) * TempFactor;
		Distance.y = pVecAccel->y * ftime_step * InvFriction + (pVecSpeed->y - pVecAccel->y * InvFriction) * TempFactor;
		Distance.z = pVecAccel->z * ftime_step * InvFriction + (pVecSpeed->z - pVecAccel->z * InvFriction) * TempFactor;
		
		TempFactor = exp(-Friction * ftime_step);
		
  		pVecSpeed->x = pVecAccel->x * InvFriction + (pVecSpeed->x - pVecAccel->x * InvFriction) * TempFactor;
  		pVecSpeed->y = pVecAccel->y * InvFriction + (pVecSpeed->y - pVecAccel->y * InvFriction) * TempFactor;
  		pVecSpeed->z = pVecAccel->z * InvFriction + (pVecSpeed->z - pVecAccel->z * InvFriction) * TempFactor;
	}
	
	return fvec_length(&Distance);
}

//

void _FVEC_startup()
{
	fnullvector = sys_malloc(sizeof(FVECTOR));
	
	fnullvector->x = 0;
	fnullvector->y = 0;
	fnullvector->z = 0;
	
	//
	
	FVEC_INITIALIZED = 1;
}

#endif