def get_signed_angle_between_degs(self, reference_vector, other_vector,
normal_vector):
unsigned_angle = Mat.Mat().angle_between_degrees(
reference_vector, other_vector)
sign_measure = Mat.Mat().dot_product(
np.cross(reference_vector, other_vector), normal_vector)
if sign_measure >= 0:
sign = 1
else:
sign = -1
return unsigned_angle * sign
def project_on_to_plane(self, vector, plane_normal):
plane_normal_length = math.sqrt(plane_normal[0]**2 +
plane_normal[1]**2 +
plane_normal[2]**2)
if plane_normal_length < 0:
raise Exception("Plane normal can not be zero")
n = Mat.Mat().normalization(plane_normal)
vector = Mat.Mat().normalization(vector)
dott = Mat.Mat().dot_product(vector, plane_normal)
result = [x - y for x, y in zip(vector, np.dot(n, dott))]
return Mat.Mat().normalization(result)
def gen_perpendicular_vector_quick(self, vector):
if abs(vector[1]) < 0.99:
perp = [vector[2], 0, vector[0]]
else:
perp = [vector[2], 0, -vector[1]]
return Mat.Mat().normalization(perp)
def fill_array(self, start_locations):
w, h = 3, len(start_locations)
coordinate = [[0 for x in range(w)] for y in range(h)]
# coordinate =[][len(body_part_index)]
x = []
y = []
z = []
length = [0.316, 0.088, 0.088]
for i in range(len(start_locations)):
if i >= 2:
uv = [(start_locations[i][0] - start_locations[i - 1][0]),
(start_locations[i][1] - start_locations[i - 1][1]),
(start_locations[i][2] - start_locations[i - 1][2])]
uv = Mat.Mat().normalization(uv)
scale = np.dot(uv, length[i - 2])
middle_points = [a + b for a, b in zip(start_locations[i - 1], scale)]
x.append(middle_points[0])
y.append(middle_points[1])
z.append(middle_points[2])
x.append(start_locations[i][0])
y.append(start_locations[i][1])
z.append(start_locations[i][2])
coordinate[0][:] = x
coordinate[1][:] = y
coordinate[2][:] = z
return coordinate
def solve_for_rotations(self, outer_joint_orientation, inner_joint_orientation, bone_number):
q1 = outer_joint_orientation
q2 = inner_joint_orientation
# finding the rotor that express rotation between two orientational frame(between outer and inner joint)
rotor = Util.Utils().find_rotation_quaternion(q1, q2)
if rotor[0] > 1:
rotor[0] = 0.99
if rotor[0] < -1:
rotor[0] = -0.99
needed_rotation = math.acos(rotor[0]) * 2 * (180 / np.pi)
self.rotations[bone_number] = needed_rotation * (np.pi / 180)
if needed_rotation <= self.bone_twist_limit:
# if the rotation is inside the limited
return Mat.Mat().multiply_two_quaternion(rotor, outer_joint_orientation)
else:
# the maximum allowed rotation angle
theta = (self.bone_twist_limit) * (np.pi / 180)
self.rotations[bone_number] = theta
# the rotation axis
if abs(rotor[0]) == 1:
return rotor
v1 = np.dot(rotor[1:], (1 / math.sqrt(1 - rotor[0] ** 2)))
w = math.cos(theta / 2)
x = v1[0] * math.sin(theta / 2)
y = v1[1] * math.sin(theta / 2)
z = v1[2] * math.sin(theta / 2)
return [w, x, y, z]
def find_rotation_quaternion(self, outer_quaternion, inner_quaternion):
conjucate = [
outer_quaternion[0], -outer_quaternion[1], -outer_quaternion[2],
-outer_quaternion[3]
]
# print(outer_quaternion)
length = math.sqrt(outer_quaternion[0]**2 + outer_quaternion[1]**2 +
outer_quaternion[2]**2 + outer_quaternion[3]**2)
inverse = np.dot(conjucate, (1 / length))
rotation = Mat.Mat().multiply_two_quaternion(inner_quaternion, inverse)
return rotation
def create_rotation_matrix(self, reference_direction):
if reference_direction[1] == 1.0:
reference_direction[1] -= 0.001
reference_direction = Mat.Mat().normalization(reference_direction)
m20 = reference_direction[0]
m21 = reference_direction[1]
m22 = reference_direction[2]
cross = np.cross(reference_direction, [0, 1, 0])
x_dir = Mat.Mat().normalization(cross)
m00 = x_dir[0]
m01 = x_dir[1]
m02 = x_dir[2]
cross = np.cross([m00, m01, m02], [m20, m21, m22])
y_dir = Mat.Mat().normalization(cross)
m10 = y_dir[0]
m11 = y_dir[1]
m12 = y_dir[2]
rotation_matrix = [[m00, m01, m02], [m10, m11, m12], [m20, m21, m22]]
return rotation_matrix
def get_angle_limited_uv(self, vector_to_limit, vector_base_line,
angle_limit_degs):
angle_between = Mat.Mat().angle_between_degrees(
vector_base_line, vector_to_limit)
if angle_between > angle_limit_degs:
correction_axis = Mat.Mat().normalization(
np.cross(Mat.Mat().normalization(vector_base_line),
Mat.Mat().normalization(vector_to_limit)))
return Mat.Mat().normalization(Mat.Mat().rotate_about_axis(
vector_base_line, angle_limit_degs, correction_axis))
else:
return Mat.Mat().normalization(vector_to_limit)
def dot_product(self, v1, v2):
v1 = Mat.Mat().normalization(v1)
v2 = Mat.Mat().normalization(v2)
return np.inner(v1, v2)
def backward(self,chain):
for loop in range(self.chain_length):
this_bone = chain.get_bone(loop)
this_bone_length = chain.get_bone(loop).get_length()
# If we are not working on the base bone
if loop != 0:
if this_bone.is_fix_bone() == 1:
this_bone_inner_to_outer_uv = this_bone.get_fixed_bone_direction_uv()
else:
this_bone_inner_to_outer_uv = this_bone.get_direction_uv()
prev_bone_inner_to_outer_uv = chain.get_bone(loop-1).get_direction_uv()
this_bone_joint = this_bone.get_joint()
this_bone_joint_type = this_bone_joint.get_joint_type()
if this_bone_joint_type == "BALL":
angle_between_degs = Util.Utils().get_angle_between_degs(prev_bone_inner_to_outer_uv,
this_bone_inner_to_outer_uv)
constraint_angle_degs = this_bone_joint.get_ball_joint_constraint_degs()
self.deg[loop] = angle_between_degs
if angle_between_degs > constraint_angle_degs:
this_bone_inner_to_outer_uv = Util.Utils().get_angle_limited_uv(this_bone_inner_to_outer_uv,
prev_bone_inner_to_outer_uv,
constraint_angle_degs)
self.deg[loop] = constraint_angle_degs
elif this_bone_joint_type == "GLOBAL_HINGE":
# Get the hinge rotation axis and project our inner-to-outer UV onto it
this_bone_inner_to_outer_uv = Util.Utils().project_on_to_plane(this_bone_inner_to_outer_uv,
this_bone_joint.get_hinge_rotation_axis())
# If there are joint constraints, then we must honour them...
cw_constraint_degs = -this_bone_joint.get_hinge_clockwise_constraint_degs()
acw_constraint_degs = this_bone_joint.get_hinge_anticlockwise_constraint_degs()
if not Util.Utils().approximately_equal(cw_constraint_degs,
-this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(),
0.001) and not Util.Utils().approximately_equal(
acw_constraint_degs,
this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(),
0.001):
hinge_reference_axis = this_bone_joint.get_reference_axis()
hinge_rotation_axis = this_bone_joint.get_hinge_rotation_axis()
# Get the signed angle (about the hinge rotation axis) between the hinge reference axis and the hinge-rotation aligned bone UV
signed_angle_degs = Util.Utils().get_signed_angle_between_degs(hinge_reference_axis,
this_bone_inner_to_outer_uv,
hinge_rotation_axis)
self.deg[loop] = signed_angle_degs * math.pi / 180
# Make our bone inner-to-outer UV the hinge reference axis rotated by its maximum clockwise or anticlockwise rotation as required
if signed_angle_degs > acw_constraint_degs:
this_bone_inner_to_outer_uv = Util.Utils().normalization(
Mat.Mat().rotate_about_axis(hinge_reference_axis, acw_constraint_degs,
hinge_rotation_axis))
self.deg[loop] = acw_constraint_degs * math.pi / 180
elif signed_angle_degs < cw_constraint_degs:
this_bone_inner_to_outer_uv = Util.Utils().normalization(
Mat.Mat().rotate_about_axis(hinge_reference_axis, cw_constraint_degs,
hinge_rotation_axis))
self.deg[loop] = cw_constraint_degs * math.pi / 180
elif this_bone_joint_type == "LOCAL_HINGE":
# Transform the hinge rotation axis to be relative to the previous bone in the chain
hinge_rotation_axis = this_bone_joint.get_hinge_rotation_axis()
m = Util.Utils().create_rotation_matrix(prev_bone_inner_to_outer_uv)
relative_hinge_rotation_axis = Util.Utils().normalization(
Util.Utils().times(m, hinge_rotation_axis))
this_bone_inner_to_outer_uv = Util.Utils().project_on_to_plane(this_bone_inner_to_outer_uv,
relative_hinge_rotation_axis)
# Constrain rotation about reference axis if required
cw_constraint_degs = -this_bone_joint.get_hinge_clockwise_constraint_degs()
acw_constraint_degs = this_bone_joint.get_hinge_anticlockwise_constraint_degs()
if not Util.Utils().approximately_equal(cw_constraint_degs,
-this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(),
0.001) and not Util.Utils().approximately_equal(
acw_constraint_degs,
this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(),
0.001):
relative_hinge_reference_axis = Util.Utils().normalization(
Util.Utils().times(m, this_bone_joint.get_reference_axis()))
signed_angle_degs = Util.Utils().get_signed_angle_between_degs(
relative_hinge_reference_axis,
this_bone_inner_to_outer_uv,
relative_hinge_rotation_axis)
self.deg[loop] = signed_angle_degs * math.pi / 180
if signed_angle_degs > acw_constraint_degs:
this_bone_inner_to_outer_uv = Util.Utils().normalization(
Mat.Mat().rotate_about_axis(relative_hinge_reference_axis, acw_constraint_degs,
relative_hinge_rotation_axis))
self.deg[loop] = acw_constraint_degs * math.pi / 180
elif signed_angle_degs < cw_constraint_degs:
this_bone_inner_to_outer_uv = Util.Utils().normalization(
Mat.Mat().rotate_about_axis(relative_hinge_reference_axis, cw_constraint_degs,
relative_hinge_rotation_axis))
self.deg[loop] = cw_constraint_degs * math.pi / 180
# twisted = np.cross(this_bone_inner_to_outer_uv,relative_hinge_rotation_axis)
# print("bone"+str(loop))
# print(twisted)
scale = [i * this_bone_length for i in this_bone_inner_to_outer_uv]
start_location = this_bone.get_start_point_position()
new_end_location = [x + y for x, y in zip(start_location, scale)]
this_bone.set_end_point_position(new_end_location)
if loop < self.chain_length - 1:
chain.get_bone(loop+1).set_start_point_position(new_end_location)
# If we ARE working on the basebone...
else:
chain.get_bone(0).set_start_point_position(self.fixed_base_location)
if self.is_base_bone_fixed == 1:
chain.get_bone(0).set_end_point_position(self.fixed_base_location_2)
if self.chain_length > 1:
chain.get_bone(1).set_start_point_position(self.fixed_base_location_2)
else:
this_bone_joint = this_bone.get_joint()
this_bone_joint_type = this_bone_joint.get_joint_type()
if this_bone_joint_type == "GLOBAL_HINGE":
hinge_rotation_axis = this_bone_joint.get_hinge_rotation_axis()
cw_constraint_degs = -this_bone_joint.get_hinge_clockwise_constraint_degs()
acw_constraint_degs = this_bone_joint.get_hinge_anticlockwise_constraint_degs()
this_bone_inner_to_outer_uv = Util.Utils().project_on_to_plane(this_bone.get_direction_uv(),
hinge_rotation_axis)
# If we have a global hinge which is not freely rotating then we must constrain about the reference axis
if not Util.Utils().approximately_equal(cw_constraint_degs,
-this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(),
0.001) and not Util.Utils().approximately_equal(
acw_constraint_degs,
this_bone_joint.get_MAX_CONSTRAINT_ANGLE_DEGS(),
0.001):
hinge_reference_axis = this_bone_joint.get_reference_axis()
signed_angle_degs = Util.Utils().get_signed_angle_between_degs(hinge_reference_axis,
this_bone_inner_to_outer_uv,
hinge_rotation_axis)
self.deg[loop] = signed_angle_degs * math.pi / 180
if signed_angle_degs > acw_constraint_degs:
this_bone_inner_to_outer_uv = Util.Utils().normalization(
Mat.Mat().rotate_about_axis(hinge_reference_axis, acw_constraint_degs,
hinge_rotation_axis))
self.deg[loop] = acw_constraint_degs * math.pi / 180
elif signed_angle_degs < cw_constraint_degs:
this_bone_inner_to_outer_uv = Util.Utils().normalization(
Mat.Mat().rotate_about_axis(hinge_reference_axis, cw_constraint_degs,
hinge_rotation_axis))
self.deg[loop] = cw_constraint_degs * math.pi / 180
# twisted = np.cross(this_bone_inner_to_outer_uv, hinge_rotation_axis)
# print("bone" + str(loop))
# print(twisted)
scale = [i * this_bone_length for i in this_bone_inner_to_outer_uv]
start_location = this_bone.get_start_point_position()
new_end_location = [x + y for x, y in zip(start_location, scale)]
this_bone.set_end_point_position(new_end_location)
if self.chain_length > 1:
chain.get_bone(1).set_start_point_position(new_end_location)
if this_bone_joint_type == "BALL":
this_bone_inner_to_outer_uv = this_bone.get_direction_uv()
angle_between_degs = Util.Utils().get_angle_between_degs(self.base_bone_constraint_uv,
this_bone_inner_to_outer_uv)
constraint_angle_degs = this_bone.get_ball_joint_constraint_degs()
self.deg[loop] = angle_between_degs * math.pi / 180
if angle_between_degs > constraint_angle_degs:
this_bone_inner_to_outer_uv = Util.Utils().get_angle_limited_uv(this_bone_inner_to_outer_uv,
self.base_bone_constraint_uv,
constraint_angle_degs)
self.deg[loop] = constraint_angle_degs * math.pi / 180
scale = [i * this_bone_length for i in this_bone_inner_to_outer_uv]
start_location = this_bone.get_start_point_position()
new_end_location = [x + y for x, y in zip(start_location, scale)]
this_bone.set_end_point_position(new_end_location)
if self.chain_length > 1:
chain.get_bone(1).set_start_point_position(new_end_location)
else:
this_bone_inner_to_outer_uv = this_bone.get_direction_uv()
scale = [i * this_bone_length for i in this_bone_inner_to_outer_uv]
start_location = this_bone.get_start_point_position()
new_end_location = [x + y for x, y in zip(start_location, scale)]
this_bone.set_end_point_position(new_end_location)
if self.chain_length > 1:
chain.get_bone(1).set_start_point_position(new_end_location)
return chain