3D Vector utils for Python

Vector3D.py

#!/usr/bin/env python
# coding: utf8

"""Vector3D.py: Usefull 3D Vector utils."""

import math

__author__      = "Kevin Gliewe aka KillerGoldFisch"
__copyright__   = "Copyright 2014, Kevin Gliewe"
__credits__     = ["kevin Gliewe",]
__license__     = "MIT"
__version__     = "1.1.0"
__date__        = "2014-10-16"
__maintainer__  = "Kevin Gliewe"
__email__       = "kevingliewe@gmail,com"
__status__      = "Testing"

"""
Changelog:
    - v 1.1.0: 2014-10-16
        Added BezierPoints calculations
    - v 1.0.1: 2014-06-24
        Added trilaterate
"""


def _isVector3D(obj):
    if type(obj).__name__=='instance':
        if obj.__class__.__name__=='Vector3D':
            return True
    return False


class Vector3D:

    def __init__(self,x=0.0,y=0.0,z=0.0):
        self.x = float(x)
        self.y = float(y)
        self.z = float(z)

    def __add__(self, other):  # on self + other
        if _isVector3D(other):
            return Vector3D(self.x + other.x, self.y + other.y, self.z + other.z)
        else:
            return self.len() + other

    def __sub__(self, other):	# on self - other
        if _isVector3D(other):
            return Vector3D(self.x - other.x, self.y - other.y, self.z - other.z)
        else:
            return self.len() - other

    def __mul__(self, other):	#on self * other
        if _isVector3D(other):
            return Vector3D(self.x * other.x, self.y * other.y, self.z * other.z)
        else:
            return Vector3D(self.x * other, self.y * other, self.z * other)

    def __div__(self, other):   #on self / other
        if _isVector3D(other):
            return Vector3D(self.x / other.x, self.y / other.y, self.z / other.z)
        else:
            return Vector3D(self.x / other, self.y / other, self.z / other)


    def __iadd__(self, other):  #on self += other
        if _isVector3D(other):
            self.x += other.x
            self.y += other.y
            self.z += other.z
        else:
            self.x += other
            self.y += other
            self.z += other
        return self


    def __isub__(self, other):  #on self += other
        if _isVector3D(other):
            self.x -= other.x
            self.y -= other.y
            self.z -= other.z
        else:
            self.x -= other
            self.y -= other
            self.z -= other
        return self


    def __imul__(self, other):	#on self * other
        if _isVector3D(other):
            self.x *= other.x
            self.y *= other.y
            self.z *= other.z
        else:
            self.x *= other
            self.y *= other
            self.z *= other
        return self

    def __idiv__(self, other):   #on self / other
        if _isVector3D(other):
            self.x /= other.x
            self.y /= other.y
            self.z /= other.z
        else:
            self.x /= other
            self.y /= other
            self.z /= other
        return self


    def __cmp__(self, other):   #on compare other
        self_len = self.len()
        other_len = 0.0
        if _isVector3D(other):
            other_len = other.len()
        else:
            other_len = other

        if self_len == other_len: return 0
        if self_len < other_len: return -1
        if self_len > other_len: return 1

    def __str__( self ):
        return self.toString()
    def __repr__(self):
        return self.__str__()

    def toString(self,roundn = 3):
        return "[x="+str(round(self.x,roundn))+", y="+str(round(self.y,roundn))+", z="+str(round(self.z,roundn))+"]"

    	# overload []
    def __getitem__(self, index):
        if index == 0:
            return self.x
        elif index == 1:
            return self.y
        elif index == 2:
            return self.z
        raise Error("out of index!")

    # overload set []
    def __setitem__(self, key, item):
            print "__setitem__"
            self.data[key] = item

    def cross(self, other):
        a = self
        b = other
        return Vector3D(
            a.y*b.z - a.z*b.y,
            a.z*b.x - a.x*b.z,
            a.x*b.y - a.y*b.x
        )

    def dot(self, other):
        return self.x*other.x+self.y*other.y+self.z*other.z

    def get(self):
        return self.x, self.y, self.z


    def len(self):
        return math.sqrt(self.x**2 + self.y**2 + self.z**2)

    def unit(self):
        return self / self.len()


    def rotateX(self, theta):
        newv = Vector3D()
        newv.x = self.x
        newv.y = self.y * math.cos(theta) + self.z * -math.sin(theta)
        newv.z = self.y * math.sin(theta) + self.z * math.cos(theta)
        return newv

    def rotateY(self, theta):
        newv = Vector3D()
        newv.x = self.x * math.cos(theta) + self.z * math.sin(theta)
        newv.y = self.y
        newv.z = self.z * -math.sin(theta) + self.z * math.cos(theta)
        return newv

    def rotateZ(self, theta):
        newv = Vector3D()
        newv.x = self.x * math.cos(theta) + self.y * -math.sin(theta)
        newv.y = self.x * math.sin(theta) + self.y * math.cos(theta)
        newv.z = self.z
        return newv


    def rotateAroundVector(self, vect2, theta):
        newv = Vector3D()
        unit = self.unit()

        q0 = math.cos(theta/2.0);
        q1 = math.sin(theta/2.0)*unit.x
        q2 = math.sin(theta/2.0)*unit.y
        q3 = math.sin(theta/2.0)*unit.z

        # column vect
        newv.x =   (q0*q0 + q1*q1 - q2*q2 - q3*q3)*self.x +               2.0*(q2*q1 - q0*q3) * self.y +               2.0*(q3*q1 + q0*q2) * self.z
        newv.y =               2.0*(q1*q2 + q0*q3)*self.x +   (q0*q0 - q1*q1 + q2*q2 - q3*q3) * self.y +               2.0*(q3*q2 - q0*q1) * self.z
        newv.z =               2.0*(q1*q3 - q0*q2)*self.x +               2.0*(q2*q3 + q0*q1) * self.y +   (q0*q0 - q1*q1 - q2*q2 + q3*q3) * self.z
        return newv

    def rotateGL(self, i = 0.0, j = 0.0, k = 0.0):
        newv = Vector3D()
        vy = Vector3D(y=1.0)
        vz = Vector3D(z=1.0)


        newv = self.rotateX(i)
        vy = vy.rotateX(i)
        vz = vz.rotateX(i)

        newv = newv.rotateAroundVector(vy,j)
        vz = vz.rotateAroundVector(vy,j)

        return newv.rotateAroundVector(vz,k)

# Find the intersection of three spheres
# P1,P2,P3 are the centers, r1,r2,r3 are the radii
# Implementaton based on Wikipedia Trilateration article.
# http://en.wikipedia.org/wiki/Trilateration
# Based on http://stackoverflow.com/questions/1406375/finding-intersection-points-between-3-spheres
def trilaterate(P1,P2,P3,r1,r2,r3):
    temp1 = P2-P1
    #print "temp1 = ", temp1
    e_x = temp1.unit()
    #print "e_x = ", e_x
    temp2 = P3-P1
    #print "temp2 = ", temp2
    i = e_x.dot(temp2)
    #print "i = ", i
    temp3 = temp2 - e_x*i
    #print "temp3 = ", temp3
    e_y = temp3.unit()
    #print "e_y = ", e_y
    e_z = e_x.cross(e_y)
    #print "e_z = ", e_z
    d = (P2-P1).len()
    #print "d = ", d
    j = e_y.dot(temp2)
    #print "j = ", j
    x = (r1*r1 - r2*r2 + d*d) / (2*d)
    #print "x = ", x
    y = (r1*r1 - r3*r3 -2*i*x + i*i + j*j) / (2*j)
    #print "y = ", y
    temp4 = r1*r1 - x*x - y*y
    #print "temp4 = ", temp4
    if temp4<0:
        raise Exception("The three sphereses do not intersect!");
    z = math.sqrt(temp4)
    #print "z = ", z
    p_12_a = P1 + e_x*x + e_y*y + e_z*z
    p_12_b = P1 + e_x*x + e_y*y - e_z*z

    return p_12_a,p_12_b


def CalculateBezierPoint(t, p0, p1, p2, p3):
    """
    See: http://devmag.org.za/2011/04/05/bzier-curves-a-tutorial/
    :param t: 0 -> 1
    :type t: float
    :param p0: 1. Point
    :type p0: Vector3D
    :param p1: 2. Point
    :type p1: Vector3D
    :param p2: 3. Point
    :type p2: Vector3D
    :param p3: 4. Point
    :type p3: Vector3D
    :return: Lerped Point
    :rtype: Vector3D
    """
    u = 1.0 - t
    tt = t * t
    uu = u * u
    uuu = uu * u
    ttt = tt * t

    p = p0 * uuu #irst term
    p += p1 * 3 * uu * t  #second term
    p += p2 * 3 * u * tt #third term
    p += p3 * ttt #fourth term

    return p

def CalculateBezierPoints(t, p0, p1, p2, p3):
    """
    See: http://devmag.org.za/2011/04/05/bzier-curves-a-tutorial/
    :param t: Count of points
    :type t: int
    :param p0: 1. Point
    :type p0: Vector3D
    :param p1: 2. Point
    :type p1: Vector3D
    :param p2: 3. Point
    :type p2: Vector3D
    :param p3: 4. Point
    :type p3: Vector3D
    :return: Lerped Point
    :rtype: lisf of [Vector3D]
    """
    for n in range(t):
        yield CalculateBezierPoint(float(n)/t, p0, p1, p2, p3)

def __test__():
    v3 = Vector3D
    p1 = v3(90.0,0.0,1.0)
    p2 = v3(-90.0,0.0,0.0)
    p3 = v3(0.0,90.0,0.0)

    print trilaterate(p1,p2,p3, 100.0,100.0,100.0)