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)