forked from RLBot/RLBotPythonExample
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathorientation.py
More file actions
47 lines (38 loc) · 1.64 KB
/
orientation.py
File metadata and controls
47 lines (38 loc) · 1.64 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
import math
from util.vec import Vec3
# This is a helper class for calculating directions relative to your car. You can extend it or delete if you want.
class Orientation:
"""
This class describes the orientation of an object from the rotation of the object.
Use this to find the direction of cars: forward, right, up.
It can also be used to find relative locations.
"""
def __init__(self, rotation):
self.yaw = float(rotation.yaw)
self.roll = float(rotation.roll)
self.pitch = float(rotation.pitch)
cr = math.cos(self.roll)
sr = math.sin(self.roll)
cp = math.cos(self.pitch)
sp = math.sin(self.pitch)
cy = math.cos(self.yaw)
sy = math.sin(self.yaw)
self.forward = Vec3(cp * cy, cp * sy, sp)
self.right = Vec3(cy*sp*sr-cr*sy, sy*sp*sr+cr*cy, -cp*sr)
self.up = Vec3(-cr*cy*sp-sr*sy, -cr*sy*sp+sr*cy, cp*cr)
# Sometimes things are easier, when everything is seen from your point of view.
# This function lets you make any location the center of the world.
# For example, set center to your car's location and ori to your car's orientation, then the target will be
# relative to your car!
def relative_location(center: Vec3, ori: Orientation, target: Vec3) -> Vec3:
"""
Returns target as a relative location from center's point of view, using the given orientation. The components of
the returned vector describes:
* x: how far in front
* y: how far right
* z: how far above
"""
x = (target - center).dot(ori.forward)
y = (target - center).dot(ori.right)
z = (target - center).dot(ori.up)
return Vec3(x, y, z)