51 lines
2.5 KiB
C
51 lines
2.5 KiB
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_NONFINITE);
|
|
input.x = 0.0; input.y = 0.0; input.z = 0.0;
|
|
assert(sim_posemath_normalize(&input, &output) == SIM_POSEMATH_ERR_DEGENERATE);
|
|
|
|
{
|
|
sim_posemath_pose start = {{0.0, 0.0, 0.0}, 0, 0, 0, 0, 0, 0};
|
|
sim_posemath_pose finish = {{2.0, 0.0, 0.0}, 1, 2, 3, 4, 5, 6};
|
|
sim_posemath_pose point = {0};
|
|
sim_posemath_segment segment = {start, finish, {0, 0, 0}, {0, 0, 1}, 1.0, 2.0, SIM_POSEMATH_LINEAR, 0};
|
|
sim_tp planner = {0};
|
|
sim_tp_state first = {0};
|
|
sim_tp_state second = {0};
|
|
uint32_t id = 0;
|
|
assert(sim_posemath_pose_line_point(&start, &finish, 0.5, &point) == SIM_POSEMATH_OK);
|
|
assert(point.tran.x == 1.0 && point.a == 0.5 && point.w == 3.0);
|
|
assert(sim_posemath_segment_validate(&segment) == SIM_POSEMATH_OK);
|
|
assert(sim_posemath_segment_point(&segment, 0.5, &point) == SIM_POSEMATH_OK);
|
|
assert(sim_tp_init(&planner, 0.1, 10.0, 10.0) == SIM_POSEMATH_OK);
|
|
assert(sim_tp_enqueue(&planner, &segment, &id) == SIM_POSEMATH_OK && id == 1);
|
|
assert(sim_tp_step(&planner) == SIM_POSEMATH_OK);
|
|
assert(sim_tp_read_state(&planner, &first) == SIM_POSEMATH_OK);
|
|
assert(sim_tp_state_hash(&planner) == sim_posemath_hash_state(&first));
|
|
assert(sim_tp_read_state(&planner, &second) == SIM_POSEMATH_OK);
|
|
assert(sim_posemath_hash_state(&first) == sim_posemath_hash_state(&second));
|
|
segment.motion_type = 99;
|
|
assert(sim_posemath_segment_validate(&segment) == SIM_POSEMATH_ERR_MOTION_TYPE);
|
|
segment.motion_type = SIM_POSEMATH_LINEAR;
|
|
segment.velocity = 11.0;
|
|
assert(sim_tp_enqueue(&planner, &segment, 0) == SIM_POSEMATH_ERR_RANGE);
|
|
}
|
|
return 0;
|
|
}
|