24 lines
972 B
C
24 lines
972 B
C
#include "sim_posemath.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
|
|
int main(void) {
|
|
sim_posemath_cartesian input = {3.0, 4.0, 0.0};
|
|
sim_posemath_cartesian output = {0};
|
|
sim_posemath_cartesian end = {10.0, 10.0, 10.0};
|
|
double dot = 0;
|
|
|
|
assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_OK);
|
|
assert(fabs(output.x - 0.6) < 1e-12 && fabs(output.y - 0.8) < 1e-12);
|
|
assert(sim_posemath_dot(&input, &end, &dot) == SIM_POSEMATH_OK && dot == 70.0);
|
|
assert(sim_posemath_line_point(&input, &end, 0.5, &output) == SIM_POSEMATH_OK);
|
|
assert(output.x == 6.5 && output.y == 7.0 && output.z == 5.0);
|
|
assert(sim_posemath_line_point(&input, &end, 1.1, &output) == SIM_POSEMATH_ERR_RANGE);
|
|
input.x = NAN;
|
|
assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_ARGUMENT);
|
|
input.x = 0.0; input.y = 0.0; input.z = 0.0;
|
|
assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_DEGENERATE);
|
|
return 0;
|
|
}
|