parent
2fa3044acb
commit
0bdae1f775
3 changed files with 137 additions and 13 deletions
@ -0,0 +1,123 @@ |
||||
import math |
||||
|
||||
class Vector3D: |
||||
def __init__(self, vector): |
||||
self.x = vector[0] |
||||
self.y = vector[1] |
||||
self.z = vector[2] |
||||
|
||||
@property |
||||
def xz(self): |
||||
return Vector3D((self.x, 0, self.z)) |
||||
|
||||
def tuple(self): |
||||
return (self.x, self.y, self.z) |
||||
|
||||
def __getitem__(self, key): |
||||
return self.tuple()[key] |
||||
|
||||
def length(self): |
||||
return math.hypot(self.x, self.y, self.z) |
||||
|
||||
def normalized(self): |
||||
x = self.x / self.length() |
||||
y = self.y / self.length() |
||||
z = self.z / self.length() |
||||
return Vector3D((x, y, z)) |
||||
|
||||
def dot(self, other): |
||||
return self.x * other.x + self.y * other.y + self.z * other.z |
||||
|
||||
def cross(self, other): |
||||
a1, a2, a3 = self.tuple() |
||||
b1, b2, b3 = other.tuple() |
||||
return Vector3D((a2*b3-a3*b2, a3*b1-a1*b3, a1*b2-a2*b1)) |
||||
|
||||
def angleDeg(self, other): |
||||
ratio = self.dot(other) / (self.length() * other.length()) |
||||
rads = math.acos(ratio) |
||||
return math.degrees(rads) |
||||
|
||||
def signedAngleDeg(self, other, ref): |
||||
angle1 = self.angleDeg(other) |
||||
cross = self.cross(ref) |
||||
angle2 = other.angleDeg(cross) |
||||
if angle2 < 90: |
||||
return -angle1 |
||||
else: |
||||
return angle1 |
||||
|
||||
def __repr__(self): |
||||
return 'Vector3D(x={}, y={}, z={})'.format(self.x, self.y, self.z) |
||||
def __str__(self): |
||||
return '[{}, {}, {}]'.format(self.x, self.y, self.z) |
||||
|
||||
class Point3D: |
||||
def __init__(self, point): |
||||
self.x = point[0] |
||||
self.y = point[1] |
||||
self.z = point[2] |
||||
|
||||
def __sub__(self, other): |
||||
#x = other.x - self.x |
||||
#y = other.y - self.y |
||||
#z = other.z - self.z |
||||
x = self.x - other.x |
||||
y = self.y - other.y |
||||
z = self.z - other.z |
||||
return Vector3D((x, y, z)) |
||||
|
||||
def tuple(self): |
||||
return (self.x, self.y, self.z) |
||||
|
||||
def __getitem__(self, key): |
||||
return self.tuple()[key] |
||||
|
||||
def __repr__(self): |
||||
return 'Point3D(x={}, y={}, z={})'.format(self.x, self.y, self.z) |
||||
def __str__(self): |
||||
return '({}, {}, {})'.format(self.x, self.y, self.z) |
||||
|
||||
if __name__ == '__main__': |
||||
# test to make sure our Vector module is the same as Panda3D |
||||
|
||||
from panda3d.core import LPoint3f, LVector3f |
||||
import random |
||||
|
||||
pPITCH_ANGLE_DIR = LVector3f(x=0, y=1, z=0) |
||||
pYAW_ANGLE_DIR = LVector3f(x=0, y=0, z=-1) |
||||
pYAW_ANGLE_REF = LVector3f(x=0, y=1, z=0) |
||||
|
||||
PITCH_ANGLE_DIR = Vector3D((0, 1, 0)) |
||||
YAW_ANGLE_DIR = Vector3D((0, 0, -1)) |
||||
YAW_ANGLE_REF = Vector3D((0, 1, 0)) |
||||
|
||||
for _ in range(1000): |
||||
r = lambda: random.uniform(-10, 10) |
||||
a, b, c = r(), r(), r() |
||||
plook_at_d = LVector3f(x=a, y=b, z=c) |
||||
look_at_d = Vector3D((a, b, c)) |
||||
|
||||
ptarget_pitch = plook_at_d.normalized().angleDeg(pPITCH_ANGLE_DIR) |
||||
target_pitch = look_at_d.normalized().angleDeg(PITCH_ANGLE_DIR) |
||||
|
||||
if round(ptarget_pitch) != round(target_pitch): |
||||
print('mismatch:', ptarget_pitch, target_pitch) |
||||
break |
||||
else: # for |
||||
print('no mismatches') |
||||
|
||||
for _ in range(1000): |
||||
r = lambda: random.uniform(-10, 10) |
||||
a, b, c = r(), r(), r() |
||||
plook_at_d = LVector3f(x=a, y=b, z=c) |
||||
look_at_d = Vector3D((a, b, c)) |
||||
|
||||
ptarget_yaw = plook_at_d.normalized().signedAngleDeg(other=pYAW_ANGLE_DIR, ref=pYAW_ANGLE_REF) |
||||
target_yaw = look_at_d.normalized().signedAngleDeg(other=YAW_ANGLE_DIR, ref=YAW_ANGLE_REF) |
||||
|
||||
if round(ptarget_yaw) != round(target_yaw): |
||||
print('mismatch:', ptarget_yaw, target_yaw) |
||||
break |
||||
else: # for |
||||
print('no mismatches') |
Loading…
Reference in new issue