minecraft-bot/mosfet/vector.py

import math
from mosfet import utils

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 utils.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')