63 lines
2.4 KiB
C
63 lines
2.4 KiB
C
#include "sim_posemath.h"
|
|
|
|
#include "posemath.h"
|
|
|
|
#include <math.h>
|
|
|
|
static int finite_cart(const sim_posemath_cartesian *value) {
|
|
return value != 0 && isfinite(value->x) && isfinite(value->y) && isfinite(value->z);
|
|
}
|
|
|
|
static PmCartesian to_pm(const sim_posemath_cartesian *value) {
|
|
PmCartesian result = {value->x, value->y, value->z};
|
|
return result;
|
|
}
|
|
|
|
static void from_pm(const PmCartesian *value, sim_posemath_cartesian *output) {
|
|
output->x = value->x;
|
|
output->y = value->y;
|
|
output->z = value->z;
|
|
}
|
|
|
|
int sim_posemath_is_finite(const sim_posemath_cartesian *value) {
|
|
return finite_cart(value);
|
|
}
|
|
|
|
int sim_posemath_normalize(const sim_posemath_cartesian *input, sim_posemath_cartesian *output) {
|
|
PmCartesian source;
|
|
PmCartesian result;
|
|
double magnitude_squared = 0.0;
|
|
if (!finite_cart(input) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
|
source = to_pm(input);
|
|
if (pmCartMagSq(&source, &magnitude_squared) != 0 || !isfinite(magnitude_squared)) return SIM_POSEMATH_ERR_NONFINITE;
|
|
if (magnitude_squared <= 1e-24) return SIM_POSEMATH_ERR_DEGENERATE;
|
|
if (pmCartUnit(&source, &result) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
|
|
from_pm(&result, output);
|
|
return SIM_POSEMATH_OK;
|
|
}
|
|
|
|
int sim_posemath_dot(const sim_posemath_cartesian *left, const sim_posemath_cartesian *right, double *output) {
|
|
PmCartesian lhs;
|
|
PmCartesian rhs;
|
|
if (!finite_cart(left) || !finite_cart(right) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
|
lhs = to_pm(left);
|
|
rhs = to_pm(right);
|
|
if (pmCartCartDot(&lhs, &rhs, output) != 0 || !isfinite(*output)) return SIM_POSEMATH_ERR_NONFINITE;
|
|
return SIM_POSEMATH_OK;
|
|
}
|
|
|
|
int sim_posemath_line_point(const sim_posemath_cartesian *start, const sim_posemath_cartesian *end,
|
|
double fraction, sim_posemath_cartesian *output) {
|
|
PmCartesian begin;
|
|
PmCartesian finish;
|
|
PmCartesian result;
|
|
PmCartesian delta;
|
|
if (!finite_cart(start) || !finite_cart(end) || output == 0) return SIM_POSEMATH_ERR_ARGUMENT;
|
|
if (!isfinite(fraction) || fraction < 0.0 || fraction > 1.0) return SIM_POSEMATH_ERR_RANGE;
|
|
begin = to_pm(start);
|
|
finish = to_pm(end);
|
|
if (pmCartCartSub(&finish, &begin, &delta) != 0 || pmCartScalMult(&delta, fraction, &delta) != 0 || pmCartCartAdd(&begin, &delta, &result) != 0) return SIM_POSEMATH_ERR_DEGENERATE;
|
|
from_pm(&result, output);
|
|
return SIM_POSEMATH_OK;
|
|
}
|