-
Notifications
You must be signed in to change notification settings - Fork 298
Expand file tree
/
Copy pathmotion_math_test.cpp
More file actions
69 lines (59 loc) · 2.09 KB
/
Copy pathmotion_math_test.cpp
File metadata and controls
69 lines (59 loc) · 2.09 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
/*
* SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD
*
* SPDX-License-Identifier: MIT
*/
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <stackchan/motion/motion_math.h>
namespace {
using stackchan::motion::MotionAngles;
using stackchan::motion::calculateNormalizedLookAngles;
using stackchan::motion::calculatePointLookAngles;
using stackchan::motion::mapNormalizedValueToAngle;
void expectEqual(int actual, int expected, const char* label)
{
if (actual != expected) {
std::cerr << label << ": expected " << expected << ", got " << actual << '\n';
std::exit(1);
}
}
void expectAngles(MotionAngles actual, MotionAngles expected, const char* label)
{
expectEqual(actual.yaw, expected.yaw, label);
expectEqual(actual.pitch, expected.pitch, label);
}
void testNormalizedMapping()
{
expectEqual(mapNormalizedValueToAngle(-1.0f, -1280, 1280), -1280, "normalized min");
expectEqual(mapNormalizedValueToAngle(0.0f, -1280, 1280), 0, "normalized center");
expectEqual(mapNormalizedValueToAngle(1.0f, -1280, 1280), 1280, "normalized max");
expectEqual(mapNormalizedValueToAngle(2.0f, -1280, 1280), 1280, "normalized clamp high");
expectEqual(mapNormalizedValueToAngle(-2.0f, 30, 870), 30, "normalized clamp low");
}
void testNormalizedLookAngles()
{
expectAngles(
calculateNormalizedLookAngles(0.5f, -0.5f, -1280, 1280, 30, 870),
MotionAngles{640, 240},
"normalized look"
);
}
void testPointLookAngles()
{
expectAngles(calculatePointLookAngles(1.0f, 0.0f, 0.0f), MotionAngles{0, 0}, "point forward");
expectAngles(calculatePointLookAngles(0.0f, 1.0f, 0.0f), MotionAngles{900, 0}, "point left");
expectAngles(calculatePointLookAngles(0.0f, -1.0f, 0.0f), MotionAngles{-900, 0}, "point right");
MotionAngles diagonal = calculatePointLookAngles(1.0f, 1.0f, 1.0f);
expectEqual(diagonal.yaw, 450, "point diagonal yaw");
expectEqual(diagonal.pitch, 352, "point diagonal pitch");
}
} // namespace
int main()
{
testNormalizedMapping();
testNormalizedLookAngles();
testPointLookAngles();
return 0;
}