KillerGoldFisch · May 18, 2017 10:19
diff --git a/Vector3D.py b/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)
	#!/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.yb.z - a.zb.y,
	a.zb.x - a.xb.z,
	a.xb.y - a.yb.x
	)

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

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


	def len(self):
	return math.sqrt(self.x2 + self.y2 + 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 = (q0q0 + q1q1 - q2q2 - q3q3)self.x + 2.0(q2q1 - q0q3) * self.y + 2.0(q3q1 + q0q2) self.z
	newv.y = 2.0(q1q2 + q0q3)self.x + (q0q0 - q1q1 + q2q2 - q3q3) * self.y + 2.0(q3q2 - q0q1) self.z
	newv.z = 2.0(q1q3 - q0q2)self.x + 2.0(q2q3 + q0q1) self.y + (q0q0 - q1q1 - q2q2 + q3q3) * 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 = (r1r1 - r2r2 + dd) / (2d)
	#print "x = ", x
	y = (r1r1 - r3r3 -2ix + ii + jj) / (2*j)
	#print "y = ", y
	temp4 = r1r1 - xx - 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_xx + e_yy + e_z*z
	p_12_b = P1 + e_xx + e_yy - 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)