116 lines
2.7 KiB
C++
116 lines
2.7 KiB
C++
#ifndef VECTOROPERATIONS_
|
|
#define VECTOROPERATIONS_
|
|
|
|
#include <stdint.h>
|
|
|
|
#include <cmath>
|
|
|
|
template <typename T>
|
|
class VectorOperations {
|
|
public:
|
|
virtual ~VectorOperations() {}
|
|
|
|
static void cross(const T left[], const T right[], T out[]) {
|
|
T temp[3] = {0, 0, 0};
|
|
temp[0] = left[1] * right[2] - left[2] * right[1];
|
|
temp[1] = left[2] * right[0] - left[0] * right[2];
|
|
temp[2] = left[0] * right[1] - left[1] * right[0];
|
|
out[0] = temp[0];
|
|
out[1] = temp[1];
|
|
out[2] = temp[2];
|
|
}
|
|
|
|
static T dot(const T a[], const T b[]) { return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; }
|
|
|
|
static void mulScalar(const T vector[], T scalar, T out[], uint8_t size) {
|
|
for (; size > 0; size--) {
|
|
out[size - 1] = vector[size - 1] * scalar;
|
|
}
|
|
}
|
|
|
|
static void add(const T vector1[], const T vector2[], T sum[], uint8_t size = 3) {
|
|
for (; size > 0; size--) {
|
|
sum[size - 1] = vector1[size - 1] + vector2[size - 1];
|
|
}
|
|
}
|
|
|
|
static void subtract(const T vector1[], const T vector2[], T sum[], uint8_t size = 3) {
|
|
for (; size > 0; size--) {
|
|
sum[size - 1] = vector1[size - 1] - vector2[size - 1];
|
|
}
|
|
}
|
|
|
|
static T norm(const T *vector, uint8_t size) {
|
|
T result = 0;
|
|
for (; size > 0; size--) {
|
|
result += vector[size - 1] * vector[size - 1];
|
|
}
|
|
result = sqrt(result);
|
|
return result;
|
|
}
|
|
|
|
static void normalize(const T *vector, T *normalizedVector, uint8_t size) {
|
|
mulScalar(vector, 1 / norm(vector, size), normalizedVector, size);
|
|
}
|
|
|
|
static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = nullptr) {
|
|
T max = vector[size - 1];
|
|
uint8_t foundIndex = size - 1;
|
|
|
|
for (; size > 0; size--) {
|
|
T abs = vector[size - 1];
|
|
if (abs < 0) {
|
|
abs = -abs;
|
|
}
|
|
if (abs > max) {
|
|
max = abs;
|
|
if (index != 0) {
|
|
foundIndex = size - 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (index != nullptr) {
|
|
*index = foundIndex;
|
|
}
|
|
|
|
return max;
|
|
}
|
|
|
|
static T maxValue(const T *vector, uint8_t size, uint8_t *index = nullptr) {
|
|
T max = vector[size - 1];
|
|
uint8_t foundIndex = size - 1;
|
|
|
|
for (; size > 0; size--) {
|
|
if (vector[size - 1] > max) {
|
|
max = vector[size - 1];
|
|
if (index != 0) {
|
|
foundIndex = size - 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (index != nullptr) {
|
|
*index = foundIndex;
|
|
}
|
|
|
|
return max;
|
|
}
|
|
|
|
static void copy(const T *in, T *out, uint8_t size) { mulScalar(in, 1, out, size); }
|
|
|
|
static bool isFinite(const T *inputVector, uint8_t size) {
|
|
for (uint8_t i = 0; i < size; i++) {
|
|
if (not std::isfinite(inputVector[i])) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
VectorOperations();
|
|
};
|
|
|
|
#endif /* VECTOROPERATIONS_ */
|