Files
RaDrone/Source/INS/geometry/vector.c

139 lines
2.8 KiB
C

#include "vector.h"
#include <math.h>
Vector2 normalizeV2(const Vector2* v, float gain)
{
float len = lengthV2(v);
Vector2 res = {.x = v->x / len, .y = v->y / len};
return res;
}
Vector3 normalizeV3(const Vector3* v, float gain)
{
float len = lengthV3(v);
Vector3 res = {.x = v->x / len, .y = v->y / len, .z = v->z};
return res;
}
Vector2 absV2(const Vector2* v)
{
Vector2 res = {.x = fabsf(v->x), .y = fabsf(v->y)};
return res;
}
Vector3 absV3(const Vector3* v)
{
Vector3 res = {.x = fabsf(v->x), .y = fabsf(v->y), .z = fabsf(v->z)};
return res;
}
float lengthV2(const Vector2* v)
{
return sqrtf(v->x * v->x + v->y * v->y);
}
float lengthV3(const Vector3* v)
{
return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z);
}
float lengthSquaredV2(const Vector2* v)
{
return v->x * v->x + v->y * v->y;
}
float lengthSquaredV3(const Vector3* v)
{
return v->x * v->x + v->y * v->y + v->z * v->z;
}
Vector2 limitV2(const Vector2* v, float min, float max)
{
Vector2 lim;
if (v->x < min) lim.x = min; else if (v->x > max) lim.x = max; else lim.x = v->x;
if (v->y < min) lim.y = min; else if (v->y > max) lim.y = max; else lim.y = v->y;
return lim;
}
Vector3 limitV3(const Vector3* v, float min, float max)
{
Vector3 lim;
if (v->x < min) lim.x = min; else if (v->x > max) lim.x = max; else lim.x = v->x;
if (v->y < min) lim.y = min; else if (v->y > max) lim.y = max; else lim.y = v->y;
if (v->z < min) lim.z = min; else if (v->z > max) lim.z = max; else lim.z = v->z;
return lim;
}
Vector2 powerV2(const Vector2* v, float pow)
{
Vector2 res = {.x = powf(v->x, pow), .y = powf(v->y, pow)};
return res;
}
Vector3 powerV3(const Vector3* v, float pow)
{
Vector3 res = {.x = powf(v->x, pow), .y = powf(v->y, pow), .z = powf(v->z, pow)};
return res;
}
Vector2 sumV2(const Vector2* v1, const Vector2* v2)
{
Vector2 res = {.x = v1->x + v2->x, .y = v1->y + v2->y};
return res;
}
Vector3 sumV3(const Vector3* v1, const Vector3* v2)
{
Vector3 res = {.x = v1->x + v2->x, .y = v1->y + v2->y, .z = v1->z + v2->z};
return res;
}
Vector2 diffV2(const Vector2* v1, const Vector2* v2)
{
Vector2 res = {.x = v1->x - v2->x, .y = v1->y - v2->y};
return res;
}
Vector3 diffV3(const Vector3* v1, const Vector3* v2)
{
Vector3 res = {.x = v1->x - v2->x, .y = v1->y - v2->y, .z = v1->z - v2->z};
return res;
}
Vector2 constProdV2(const Vector2* v, float value)
{
Vector2 res = {.x = v->x * value, .y = v->y * value};
return res;
}
Vector3 constProdV3(const Vector3* v, float value)
{
Vector3 res = {.x = v->x * value, .y = v->y * value, .z = v->z * value};
return res;
}
float scalarProdV2(const Vector2* v1, const Vector2* v2)
{
float res = v1->x * v2->x + v1->y * v2->y;
return res;
}
float scalarProdV3(const Vector3* v1, const Vector3* v2)
{
float res = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
return res;
}