def rotate_camera(self, event: wx.MouseEvent, orbit: bool = True) -> None:
"""Update rotate quat to reflect rotation controls.
Args:
event: A wx.MouseEvent representing an updated mouse position.
orbit: Optional; True: uses orbit controls, False: uses arcball
controls. Defaults to True.
"""
if self._mouse_pos is None:
self._mouse_pos = event.Position
return
last = self._mouse_pos
cur = event.Position
canvas_size = self._canvas.get_canvas_size()
p1x = last.x * 2.0 / canvas_size.width - 1.0
p1y = 1 - last.y * 2.0 / canvas_size.height
p2x = cur.x * 2.0 / canvas_size.width - 1.0
p2y = 1 - cur.y * 2.0 / canvas_size.height
with self._rot_lock:
if orbit:
dx = p2y - p1y
dy = p2x - p1x
pitch = glm.angleAxis(dx, vec3(-1.0, 0.0, 0.0))
yaw = glm.angleAxis(dy, vec3(0.0, 0.0, 1.0))
self._rot_quat = pitch * self._rot_quat * yaw
else:
quat = arcball(p1x, p1y, p2x, p2y, self._dist / 250.0)
self._rot_quat = quat * self._rot_quat
self._mouse_pos = cur
def __init__(self, poses:BonePoseSet) -> None:
self.poses = poses
self.animatable = Bezier2(Transformation())
self.animatable.set_target( t1=1.,
c0=Transformation( quaternion=glm.angleAxis(0.3,glm.vec3((1.,0.,0.)))),
c1=Transformation( quaternion=glm.angleAxis(0.3,glm.vec3((1.,0.,0.)))),
tgt=Transformation(quaternion=glm.angleAxis(0.3,glm.vec3((1.,0.,0.)))),
callback=self.animation_callback )
pass
def animation_callback(self, t:float) -> None:
t_sec = math.floor(t)
t_int = int(t_sec)
tgt = 1.0
if (t_int&1): tgt=-1.
self.animatable.set_target( t1=t_sec+1.,
c0=Transformation(quaternion=glm.angleAxis(0.3,glm.vec3((1.,0.,0.)))),
c1=Transformation(quaternion=glm.angleAxis(0.5,glm.vec3((0.,1.,0.)))),
tgt=Transformation(quaternion=glm.angleAxis(tgt*0.3,glm.vec3((1.,0.,0.)))),
callback=self.animation_callback )
pass
def arcball(
p1x: float, p1y: float, p2x: float, p2y: float, r: float) -> quat:
"""Return quaternion after arcball rotation.
See https://www.khronos.org/opengl/wiki/Object_Mouse_Trackball for details.
Args:
p1x: Previous screen x-coordinate, normalized.
p1y: Previous screen y-coordinate, normalized.
p2x: Current screen x-coordinate, normalized.
p2y: Current screen y-coordinate, normalized.
r: Radius of arcball.
"""
p1 = vec3(p1x, -project_to_sphere(r, p1x, p1y), p1y)
p2 = vec3(p2x, -project_to_sphere(r, p2x, p2y), p2y)
axis = glm.normalize(glm.cross(p1, p2))
# calculate angle between p1 and p2
d = map(lambda x, y: x - y, p1, p2)
t = sqrt(sum(x * x for x in d)) / (2.0 * r)
if t > 1.0:
t = 1.0
if t < -1.0:
t = -1.0
phi = 2.0 * asin(t)
return glm.angleAxis(phi, axis)
def rotation(self, orig, dest):
identityQuat = glm.quat(1.0, 0.0, 0.0, 0.0)
epsilon = 0.00001
cosTheta = glm.dot(orig, dest)
if cosTheta >= 1.0 - epsilon:
#// orig and dest point in the same direction
return identityQuat
if cosTheta < -1.0 + epsilon:
'''
// special case when vectors in opposite directions :
// there is no "ideal" rotation axis
// So guess one; any will do as long as it's perpendicular to start
// This implementation favors a rotation around the Up axis (Y),
// since it's often what you want to do.
'''
rotationAxis = glm.cross(glm.vec3(0.0, 0.0, 1.0), orig)
if glm.length(
rotationAxis
) < epsilon: # // bad luck, they were parallel, try again!
rotationAxis = glm.cross(glm.vec3(1.0, 0.0, 0.0), orig)
rotationAxis = glm.normalize(rotationAxis)
return glm.angleAxis(glm.pi(), rotationAxis)
#// Implementation from Stan Melax's Game Programming Gems 1 article
rotationAxis = glm.cross(orig, dest)
s = math.sqrt((1.0 + cosTheta) * 2.0)
invs = 1.0 / s
return glm.quat(s * 0.5, rotationAxis.x * invs, rotationAxis.y * invs,
rotationAxis.z * invs)
class Transform:
"""Class represents a full 3d transformation including scale,
translation and rotation."""
position = glm.vec3(0, 0, 0) # position as vec3
scale = glm.vec3(1, 1, 1) # scale as vec3
orientation = glm.angleAxis(0, glm.vec3(0)) # orientation as a quaternion
def __init__(self,
position=glm.vec3(0, 0, 0),
orientation=glm.angleAxis(0, glm.vec3(0)),
scale=glm.vec3(1, 1, 1)):
"""construct transform from an 4x4 transformation matrix"""
self.position = position
self.orientation = orientation
self.scale = scale
def __eq__(self, other):
return self.position == other.position \
and self.scale == other.scale \
and self.orientation == other.orientation
def __ne__(self, other):
return self.position != other.position \
or self.scale != other.scale \
or self.orientation != other.orientation
def mat4(self):
return glm.translate(glm.mat4(1.0), self.position) * glm.scale(
glm.mat4(1.0), self.scale) * glm.mat4_cast(self.orientation)
def lookAt(self, target, up):
"""set orientation to look at the target, target and up are expected to be glm.vec3"""
self.orientation = glm.quatLookAt(
glm.normalize(target - self.position), up)
def __init__(self,
position=glm.vec3(0, 0, 0),
orientation=glm.angleAxis(0, glm.vec3(0)),
scale=glm.vec3(1, 1, 1)):
"""construct transform from an 4x4 transformation matrix"""
self.position = position
self.orientation = orientation
self.scale = scale
def _gyro_update_hook(self):
if self.is_calibrating:
if self.is_calibrating < time.time():
self._set_calibration()
else:
for xyz in self.gyro:
self.calibration_acumulator += xyz
self.calibration_acumulations += 3
for gx, gy, gz in self.gyro_in_rad:
# TODO: find out why 1/86 works, and not 1/60 or 1/(60*30)
rotation \
= angleAxis(gx * (-1/86), self.direction_X) \
* angleAxis(gy * (-1/86), self.direction_Y) \
* angleAxis(gz * (-1/86), self.direction_Z)
self.direction_X *= rotation
self.direction_Y *= rotation
self.direction_Z *= rotation
self.direction_Q *= rotation
def quaternion_of_rotation(rotation:glm.Mat3) -> glm.Quat:
"""
Note that R . axis = axis
RI = (R - I*999/1000)
Then RI . axis = axis/1000
Then det(RI) is going to be 1/1000 * at most 2 * at most 2
And if na is perp to axis, then RI.na = R.na - 999/1000.na, which is perp to axis
Then |R.na| < 2|na|
If RI' . RI = I, then consider v' = RI' . v for some v=(a*axis + b*na0 + c*na1)
(a'*axis + b'*na0 + c'*na1) = RI' . (a*axis + b*na0 + c*na1)
Then RI . (a'*axis + b'*na0 + c'*na1) = (a*axis + b*na0 + c*na1)
Then a'*RI.axis + b'*RI.na0 + c'*RI.na1 = a*axis + b*na0 + c*na1
Then a'/1000*axis + b'*(R.na0-0.999.na0) + c'*(R.na1-0.999.na1) = a*axis + b*na0 + c*na1
Then a = a'/1000, and
-0.999b' + b'cos(angle) + c'sin(angle) = b, etc
If we set |v| to be 1, then |v'| must be det(RI') = 1/det(RI) > 100
If angle is not close to zero, then a' / b' >> 1
This can be repeated:
v' = normalize(RI' . v)
v'' = normalize(RI' . v')
v''' = normalize(RI' . v'') etc
This gets closer and closer to the axis
"""
rot_min_id : glm.Mat3 = rotation - (0.99999 * glm.mat3()) # type: ignore
rot_min_id_i : glm.Mat3 = glm.inverse(rot_min_id) # type: ignore
for j in range(3):
v = glm.vec3()
v[j] = 1.
for i in range(10):
last_v = v
rid_i_v : glm.Vec3 = rot_min_id_i * v # type: ignore
v = glm.normalize(rid_i_v)
pass
axis = v
dist2 = glm.length2(v - last_v) # type: ignore
if dist2<0.00001: break
pass
w = glm.vec3([1.0,0,0])
if axis[0]>0.9 or axis[0]<-0.9: w = glm.vec3([0,1.0,0])
na0 : glm.Vec3 = glm.normalize(glm.cross(w, axis))
na1 : glm.Vec3 = glm.cross(axis, na0)
# Rotate w_perp_n around the axis of rotation by angle A
na0_r : glm.Vec3 = rotation * na0 # type: ignore
na1_r : glm.Vec3 = rotation * na1 # type: ignore
# Get angle of rotation
cos_angle = glm.dot(na0, na0_r)
sin_angle = -glm.dot(na0, na1_r)
angle = math.atan2(sin_angle, cos_angle)
# Set quaternion
return glm.angleAxis(angle, axis)
def animate(self, time: float) -> None:
self.poses[0].transformation_reset()
self.poses[1].transformation_reset()
self.poses[2].transformation_reset()
angle = math.sin(time * 0.2) * 0.3
q = glm.quat()
q = glm.angleAxis(time * 0.3, glm.vec3([0, 0, 1])) * q
q = glm.angleAxis(1.85, glm.vec3([1, 0, 0])) * q
self.poses[0].transform(
Transformation(translation=(0., 0., 0.), quaternion=q))
q = glm.quat()
q = glm.angleAxis(angle * 4, glm.vec3([0, 0, 1])) * q
self.poses[1].transform(
Transformation(translation=(0., 0., -math.cos(4 * angle)),
quaternion=q))
q = glm.quat()
q = glm.angleAxis(angle * 4, glm.vec3([0, 0, 1])) * q
self.poses[2].transform(
Transformation(translation=(0., 0., +math.cos(4 * angle)),
quaternion=q))
self.pose.update(int(time * 1E5))
pass
def rotateAroundPoint(self, angle, axis, pivot):
rot = glm.angleAxis(angle, glm.normalize(axis))
vec = self.position - self.origin - pivot
self.setPosition(pivot + vec * rot + self.origin)
def rotateAngleAxis(self, angle, axis):
rot = glm.angleAxis(angle, glm.normalize(axis))
self.setRotation(self.rotation * rot)
def setRotationAngleAxis(self, angle, axis):
rot = glm.angleAxis(angle, glm.normalize(axis))
self.setRotation(rot)
def aa2q(angle, axis=glm.vec3(0, 0, 1)):
return glm.angleAxis(angle, glm.normalize(axis))
def rotate(self, axis:glm.Vec3, angle:float) -> None:
q = glm.angleAxis(angle, axis)
self.quaternion = q * self.quaternion
self.translation = q * self.translation # type: ignore
pass
def update(self, deltaTime):
# movement in camera coordinates
movement = self._currentTransform.orientation * (self._movementInput *
self.movementSpeedMod)
if self.mode == 0:
# rotate position around target
# figure out where the old target is
oldTarget = self._desiredTransform.position \
+ self._desiredTransform.orientation * glm.vec3(0, 0, -1) * self._desiredTargetDistance
# rotate the camera
self._desiredTransform.orientation = glm.normalize(
glm.angleAxis(self._rotationInput.x, self.worldUp) *
self._desiredTransform.orientation *
glm.angleAxis(self._rotationInput.y, glm.vec3(1, 0, 0)))
# move so old target matches new target
newTarget = self._desiredTransform.position \
+ self._desiredTransform.orientation * glm.vec3(0, 0, -1) * self._desiredTargetDistance
self._desiredTransform.position += oldTarget - newTarget
# pan
# zooming, make sure distance to the target does not become negative
if self._desiredTargetDistance + self._movementInput.z > 0.01 * self.zoomSpeed:
self._desiredTargetDistance += self._movementInput.z
# now just apply movement
self._desiredTransform.position += movement
# interpolate
# interpolate distance to target
self._currentTargetDistance = glm.mix(
self._currentTargetDistance, self._desiredTargetDistance,
glm.pow(deltaTime, self.movementSmoothing))
# interpolate current target position
desiredTarget = self._desiredTransform.position + self._desiredTransform.orientation * glm.vec3(
0, 0, -1) * self._desiredTargetDistance
oldActualTarget = self._currentTransform.position + self._currentTransform.orientation * glm.vec3(
0, 0, -1) * self._currentTargetDistance
oldActualTarget = glm.mix(
oldActualTarget, desiredTarget,
glm.pow(deltaTime, self.movementSmoothing))
# interpolate orientation
self._currentTransform.orientation = glm.slerp(
self._currentTransform.orientation,
self._desiredTransform.orientation,
glm.pow(deltaTime, self.rotationSmoothing))
# calculate proper position using difference that was created by rotation and moving the target
newActualTarget = self._currentTransform.position + self._currentTransform.orientation * glm.vec3(
0, 0, -1) * self._currentTargetDistance
self._currentTransform.position += oldActualTarget - newActualTarget
elif self.mode == 1:
# movement
self._desiredTransform.position += movement
# rotation
self._desiredTransform.orientation = glm.normalize(
glm.angleAxis(self._rotationInput.x, self.worldUp) *
self._desiredTransform.orientation *
glm.angleAxis(self._rotationInput.y, glm.vec3(1, 0, 0)))
# interpolate between transform and desired transform
self._currentTransform.position = glm.mix(
self._currentTransform.position,
self._desiredTransform.position,
glm.pow(deltaTime, self.movementSmoothing))
self._currentTransform.orientation = glm.slerp(
self._currentTransform.orientation,
self._desiredTransform.orientation,
glm.pow(deltaTime, self.rotationSmoothing))
self.modelMatrix = self._currentTransform.mat4()
self.viewMatrix = glm.inverse(self.modelMatrix)
self._rotationInput.x = 0
self._rotationInput.y = 0
self._movementInput.x = 0
self._movementInput.y = 0
self._movementInput.z = 0
self.movementSpeedMod = 1.0