276 lines
6.9 KiB
C++
276 lines
6.9 KiB
C++
#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
|
|
}*/
|