def intersectsBox(self, box):
p1 = vector3.Vector3()
p2 = vector3.Vector3()
planes = self.planes
for plane in planes:
p1.x = box.min.x if plane.normal.x > 0 else box.max.x
p2.x = box.max.x if plane.normal.x > 0 else box.min.x
p1.y = box.min.y if plane.normal.y > 0 else box.max.y
p2.y = box.max.y if plane.normal.y > 0 else box.min.y
p1.z = box.min.z if plane.normal.z > 0 else box.max.z
p2.z = box.max.z if plane.normal.z > 0 else box.min.z
d1 = plane.distanceToPoint(p1)
d2 = plane.distanceToPoint(p2)
# if both outside plane, no intersection
if d1 < 0 and d2 < 0:
return False
return True
def s_barycoordFromPoint(point, a, b, c, optionalTarget=None):
v0 = vector3.Vector3()
v1 = vector3.Vector3()
v2 = vector3.Vector3()
v0.subVectors(c, a)
v1.subVectors(b, a)
v2.subVectors(point, a)
dot00 = v0.dot(v0)
dot01 = v0.dot(v1)
dot02 = v0.dot(v2)
dot11 = v1.dot(v1)
dot12 = v1.dot(v2)
denom = (dot00 * dot11 - dot01 * dot01)
result = optionalTarget or vector3.Vector3()
# collinear or singular triangle
if denom == 0:
# arbitrary location outside of triangle?
# not sure if self is the best idea, maybe should be returning None
return result.set(-2, -1, -1)
invDenom = 1 / denom
u = (dot11 * dot02 - dot01 * dot12) * invDenom
v = (dot00 * dot12 - dot01 * dot02) * invDenom
# barycentric coordinates must always sum to 1
return result.set(1 - u - v, v, u)
def intersectLine(self, line, optionalTarget=None):
v1 = vector3.Vector3()
result = optionalTarget or vector3.Vector3()
direction = line.delta(v1)
denominator = self.normal.dot(direction)
if denominator == 0:
# line is coplanar, return origin
if self.distanceToPoint(line.start == 0):
return result.copy(line.start)
# Unsure if self is the correct method to handle self case.
return None
t = -(line.start.dot(self.normal) + self.constant) / denominator
if t < 0 or t > 1:
return None
return result.copy(direction).multiplyScalar(t).add(line.start)
def area(self):
v0 = vector3.Vector3()
v1 = vector3.Vector3()
v0.subVectors(self.c, self.b)
v1.subVectors(self.a, self.b)
return v0.cross(v1).length() * 0.5
def get_origin(self, cube_width): cube_width = get_mcode_cube_width(cube_width) sector_origin = self.centre - vector3.Vector3(self.radius, self.radius, self.radius) sox = math.floor(sector_origin.x) soy = math.floor(sector_origin.y) soz = math.floor(sector_origin.z) sox -= (sox - int(base_coords.x)) % cube_width soy -= (soy - int(base_coords.y)) % cube_width soz -= (soz - int(base_coords.z)) % cube_width return vector3.Vector3(float(sox), float(soy), float(soz))
def setFromCoplanarPoints(self, a, b, c):
v1 = vector3.Vector3()
v2 = vector3.Vector3()
normal = v1.subVectors(c, b).cross(v2.subVectors(a, b)).normalize()
# Q: should an error be thrown if normal is zero (e.g. degenerate plane)?
self.setFromNormalAndCoplanarPoint(normal, a)
return self
def lookAt(self, eye, target, up):
x = vector3.Vector3()
y = vector3.Vector3()
z = vector3.Vector3()
te = self.elements
z.subVectors(eye, target)
if z.lengthSq() == 0:
# eye and target are in the same position
z.z = 1
z.normalize()
x.crossVectors(up, z)
if x.lengthSq() == 0:
# up and z are parallel
if abs(up.z) == 1:
z.x += 0.0001
else:
z.z += 0.0001
z.normalize()
x.crossVectors(up, z)
x.normalize()
y.crossVectors(z, x)
te[0] = x.x
te[4] = y.x
te[8] = z.x
te[1] = x.y
te[5] = y.y
te[9] = z.y
te[2] = x.z
te[6] = y.z
te[10] = z.z
return self
def s_normal(a, b, c, optionalTarget=None):
v0 = vector3.Vector3()
result = optionalTarget or vector3.Vector3()
result.subVectors(c, b)
v0.subVectors(a, b)
result.cross(v0)
resultLengthSq = result.lengthSq()
if resultLengthSq > 0:
return result.multiplyScalar(1 / math.sqrt(resultLengthSq))
return result.set(0, 0, 0)
def calculate_from_id64(input):
# If input is a string, assume hex
if util.is_str(input):
input = int(input, 16)
# Calculate the shifts we need to do to get the individual fields out
# Can't tell how long N2 field is (or if the start moves!), assuming ~16 for now
len_used = 0
input, mc = util.unpack_and_shift(input, 3)
len_used += 3 # mc = 0-7 for a-h
input, boxel_z = util.unpack_and_shift(input, 7 - mc)
len_used += 7 - mc
input, sector_z = util.unpack_and_shift(input, 7)
len_used += 7
input, boxel_y = util.unpack_and_shift(input, 7 - mc)
len_used += 7 - mc
input, sector_y = util.unpack_and_shift(input, 6)
len_used += 6
input, boxel_x = util.unpack_and_shift(input, 7 - mc)
len_used += 7 - mc
input, sector_x = util.unpack_and_shift(input, 7)
len_used += 7
input, n2 = util.unpack_and_shift(input, 55 - len_used)
input, body_id = util.unpack_and_shift(input, 9)
# Multiply each X/Y/Z value by the cube width to get actual coords
boxel_size = 10 * (2**mc)
coord_x = (sector_x * sector.sector_size) + (boxel_x *
boxel_size) + (boxel_size / 2)
coord_y = (sector_y * sector.sector_size) + (boxel_y *
boxel_size) + (boxel_size / 2)
coord_z = (sector_z * sector.sector_size) + (boxel_z *
boxel_size) + (boxel_size / 2)
coords_internal = vector3.Vector3(coord_x, coord_y, coord_z)
# Shift the coords to be the origin we know and love
coords = coords_internal + sector.internal_origin_offset
return (coords, boxel_size, n2, body_id)
def intersectSphere(self, sphere, optionalTarget=None):
v1 = vector3.Vector3()
v1.subVectors(sphere.center, self.origin)
tca = v1.dot(self.direction)
d2 = v1.dot(v1) - tca * tca
radius2 = sphere.radius * sphere.radius
if d2 > radius2: return None
thc = math.sqrt(radius2 - d2)
# t0 = first intersect point - entrance on front of sphere
t0 = tca - thc
# t1 = second intersect point - exit point on back of sphere
t1 = tca + thc
# test to see if both t0 and t1 are behind the ray - if so, return None
if t0 < 0 and t1 < 0: return None
# test to see if t0 is behind the ray:
# if it is, the ray is inside the sphere, so return the second exit point scaled by t1,
# in order to always return an intersect point that is in front of the ray.
if t0 < 0: return self.at(t1, optionalTarget)
# else t0 is in front of the ray, so return the first collision point scaled by t0
return self.at(t0, optionalTarget)
def recast(self, t):
v1 = vector3.Vector3()
self.origin.copy(self.at(t, v1))
return self
def closestPointToPoint( self, point, clampToLine, optionalTarget = None ):
t = self.closestPointToPointParameter( point, clampToLine )
result = optionalTarget or vector3.Vector3()
return self.delta( result ).multiplyScalar( t ).add( self.start )
def s_containsPoint(point, a, b, c):
v1 = vector3.Vector3()
result = Triangle.s_barycoordFromPoint(point, a, b, c, v1)
return (result.x >= 0) and (result.y >= 0) and (
(result.x + result.y) <= 1)
def closestPointToPoint(self, point, optionalTarget=None):
p = plane.Plane()
edgeList = [line3.Line3(), line3.Line3(), line3.Line3()]
projectedPoint = vector3.Vector3()
closestPoint = vector3.Vector3()
result = optionalTarget or vector3.Vector3()
minDistance = float("inf")
# project the point onto the plane of the triangle
p.setFromCoplanarPoints(self.a, self.b, self.c)
p.projectPoint(point, projectedPoint)
# check if the projection lies within the triangle
if self.containsPoint(projectedPoint) == True:
# if so, self is the closest point
result.copy(projectedPoint)
else:
# if not, the point falls outside the triangle. the result is the closest point to the triangle"s edges or vertices
edgeList[0].set(self.a, self.b)
edgeList[1].set(self.b, self.c)
edgeList[2].set(self.c, self.a)
for i in xrange(len(edgeList)):
edgeList[i].closestPointToPoint(projectedPoint, True,
closestPoint)
distance = projectedPoint.distanceToSquared(closestPoint)
if distance < minDistance:
minDistance = distance
result.copy(closestPoint)
return result
def __init__(self, x, y, z, name=None, id64=None):
self._position = vector3.Vector3(float(x), float(y), float(z))
self._name = name
self._id = None
self._id64 = id64
self.uses_sc = False
self._hash = u"{}/{},{},{}".format(self.name, self.position.x,
self.position.y,
self.position.z).__hash__()
def closestPointToPointParameter( self, point, clampToLine ):
startP = vector3.Vector3()
startEnd = vector3.Vector3()
startP.subVectors( point, self.start )
startEnd.subVectors( self.end, self.start )
startEnd2 = startEnd.dot( startEnd )
startEnd_startP = startEnd.dot( startP )
t = startEnd_startP / startEnd2
if clampToLine:
t = _Math.clamp( t, 0, 1 )
return t
def toVector3(self, optionalResult=None):
if optionalResult is not None:
return optionalResult.set(self._x, self._y, self._z)
else:
return vector3.Vector3(self._x, self._y, self._z)
def get_boxel_origin(position, mcode):
posinput = util.get_as_position(position)
cube_width = sector.get_mcode_cube_width(mcode)
if posinput is None or cube_width is None:
return None
x = posinput.x - (
(posinput.x - sector.internal_origin_offset.x) % cube_width)
y = posinput.y - (
(posinput.y - sector.internal_origin_offset.y) % cube_width)
z = posinput.z - (
(posinput.z - sector.internal_origin_offset.z) % cube_width)
return vector3.Vector3(x, y, z)
def closestPointToPoint(self, point, optionalTarget=None):
result = optionalTarget or vector3.Vector3()
result.subVectors(point, self.origin)
directionDistance = result.dot(self.direction)
if directionDistance < 0:
return result.copy(self.origin)
return result.copy(
self.direction).multiplyScalar(directionDistance).add(self.origin)
def clampPoint(self, point, optionalTarget=None):
deltaLengthSq = self.center.distanceToSquared(point)
result = optionalTarget or vector3.Vector3()
result.copy(point)
if deltaLengthSq > (self.radius * self.radius):
result.sub(self.center).normalize()
result.multiplyScalar(self.radius).add(self.center)
return result
def applyMatrix4(self, matrix, optionalNormalMatrix=None):
v1 = vector3.Vector3()
m1 = matrix3.Matrix3()
normalMatrix = optionalNormalMatrix or m1.getNormalMatrix(matrix)
referencePoint = self.coplanarPoint(v1).applyMatrix4(matrix)
normal = self.normal.applyMatrix3(normalMatrix).normalize()
self.constant = -referencePoint.dot(normal)
return self
def applyToBufferAttribute( self, attribute ):
v1 = vector3.Vector3()
for i in xrange( attribute.count ):
v1.x = attribute.getX( i )
v1.y = attribute.getY( i )
v1.z = attribute.getZ( i )
v1.applyMatrix3( self )
attribute.setXYZ( i, v1.x, v1.y, v1.z )
return attribute
def distanceSqToPoint(self, point):
v1 = vector3.Vector3()
directionDistance = v1.subVectors(point,
self.origin).dot(self.direction)
# point behind the ray
if directionDistance < 0:
return self.origin.distanceToSquared(point)
v1.copy(self.direction).multiplyScalar(directionDistance).add(
self.origin)
return v1.distanceToSquared(point)
def _get_relpos_from_soffset(position, mcode):
row = int(position // _srp_sidelength)
position -= (row * _srp_sidelength)
stack = int(position // _srp_rowlength)
position -= (stack * _srp_rowlength)
column = position
cubeside = sector.get_mcode_cube_width(mcode)
halfwidth = cubeside / 2
approx_x = (column * cubeside) + halfwidth
approx_y = (stack * cubeside) + halfwidth
approx_z = (row * cubeside) + halfwidth
return (vector3.Vector3(approx_x, approx_y, approx_z), halfwidth)
def decompose(self, position, quaternion, scale):
vector = vector3.Vector3()
matrix = Matrix4()
te = self.elements
sx = vector.set(te[0], te[1], te[2]).length()
sy = vector.set(te[4], te[5], te[6]).length()
sz = vector.set(te[8], te[9], te[10]).length()
# if determine is negative, we need to invert one scale
det = self.determinant()
if det < 0: sx = -sx
position.x = te[12]
position.y = te[13]
position.z = te[14]
# scale the rotation part
matrix.copy(self)
invSX = 1. / sx
invSY = 1. / sy
invSZ = 1. / sz
matrix.elements[0] *= invSX
matrix.elements[1] *= invSX
matrix.elements[2] *= invSX
matrix.elements[4] *= invSY
matrix.elements[5] *= invSY
matrix.elements[6] *= invSY
matrix.elements[8] *= invSZ
matrix.elements[9] *= invSZ
matrix.elements[10] *= invSZ
quaternion.setFromRotationMatrix(matrix)
scale.x = sx
scale.y = sy
scale.z = sz
return self
def _get_system_from_pos(input, mcode, allow_ha=True):
input = util.get_as_position(input)
if input is None:
return None
psect = get_sector(input, allow_ha=allow_ha)
# Get cube width for this mcode, and the sector origin
cwidth = sector.get_mcode_cube_width(mcode)
psorig = psect.get_origin(cwidth)
# Get the relative inputition within this sector and the system identifier
relpos = vector3.Vector3(input.x - psorig.x, input.y - psorig.y,
input.z - psorig.z)
sysid = _get_sysid_from_relpos(relpos, mcode, format_output=True)
return system.PGSystemPrototype(input.x,
input.y,
input.z,
"{} {}".format(psect.name, sysid),
sector=psect,
uncertainty=0)
def __readMarkerData(self, handle):
""" Internal Method to the 3 float values that make up a marker location,
and return a vector3 with the values.
TODO: Add in order transform
Args:
handle (binaryFile) : file handle object to the open c3d file
Returns:
A vector 3 object with the X, Y and Z coordinates of the marker
"""
tempX = handle.readFloat()
tempY = handle.readFloat()
tempZ = handle.readFloat()
handle.readFloat()
newVect = vector3.Vector3(tempX, tempY, tempZ)
return newVect
def extractRotation(self, m):
v1 = vector3.Vector3()
te = self.elements
me = m.elements
scaleX = 1. / v1.setFromMatrixColumn(m, 0).length()
scaleY = 1. / v1.setFromMatrixColumn(m, 1).length()
scaleZ = 1. / v1.setFromMatrixColumn(m, 2).length()
te[0] = me[0] * scaleX
te[1] = me[1] * scaleX
te[2] = me[2] * scaleX
te[4] = me[4] * scaleY
te[5] = me[5] * scaleY
te[6] = me[6] * scaleY
te[8] = me[8] * scaleZ
te[9] = me[9] * scaleZ
te[10] = me[10] * scaleZ
return self
def centre(self): return self.origin + vector3.Vector3(sector_size / 2, sector_size / 2, sector_size / 2)
def get_origin(self, cube_width = None): ox = base_coords.x + (sector_size * self.x) oy = base_coords.y + (sector_size * self.y) oz = base_coords.z + (sector_size * self.z) return vector3.Vector3(ox, oy, oz)
