Skip to content

Why there are two camPosToQuaternion functions? #10

Description

@Kristing-Mini

In the file utilites_math.py, there exist two two camPosToQuaternion functions. I get the same input to these two functions and they return different results.
`def camPosToQuaternion(cx, cy, cz):
camDist = math.sqrt(cx * cx + cy * cy + cz * cz)
cx = cx / camDist
cy = cy / camDist
cz = cz / camDist
axis = (-cz, 0, cx)
angle = math.acos(cy)
a = math.sqrt(2) / 2
b = math.sqrt(2) / 2
w1 = axis[0]
w2 = axis[1]
w3 = axis[2]
c = math.cos(angle / 2)
d = math.sin(angle / 2)
q1 = a * c - b * d * w1
q2 = b * c + a * d * w1
q3 = a * d * w2 + b * d * w3
q4 = -b * d * w2 + a * d * w3
return (q1, q2, q3, q4)

def quaternionFromYawPitchRoll(yaw, pitch, roll):
c1 = math.cos(yaw / 2.0)
c2 = math.cos(pitch / 2.0)
c3 = math.cos(roll / 2.0)
s1 = math.sin(yaw / 2.0)
s2 = math.sin(pitch / 2.0)
s3 = math.sin(roll / 2.0)
q1 = c1 * c2 * c3 + s1 * s2 * s3
q2 = c1 * c2 * s3 - s1 * s2 * c3
q3 = c1 * s2 * c3 + s1 * c2 * s3
q4 = s1 * c2 * c3 - c1 * s2 * s3
return (q1, q2, q3, q4)

def camPosToQuaternion(cx, cy, cz):
q1a = 0
q1b = 0
q1c = math.sqrt(2) / 2
q1d = math.sqrt(2) / 2
camDist = math.sqrt(cx * cx + cy * cy + cz * cz)
cx = cx / camDist
cy = cy / camDist
cz = cz / camDist
t = math.sqrt(cx * cx + cy * cy)
tx = cx / t
ty = cy / t
yaw = math.acos(ty)
if tx > 0:
yaw = 2 * math.pi - yaw
pitch = 0
tmp = min(max(txcx + tycy, -1),1)
#roll = math.acos(tx * cx + ty * cy)
roll = math.acos(tmp)
if cz < 0:
roll = -roll
print("%f %f %f" % (yaw, pitch, roll))
q2a, q2b, q2c, q2d = quaternionFromYawPitchRoll(yaw, pitch, roll)
q1 = q1a * q2a - q1b * q2b - q1c * q2c - q1d * q2d
q2 = q1b * q2a + q1a * q2b + q1d * q2c - q1c * q2d
q3 = q1c * q2a - q1d * q2b + q1a * q2c + q1b * q2d
q4 = q1d * q2a + q1c * q2b - q1b * q2c + q1a * q2d
return (q1, q2, q3, q4)
`

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

      Milestone

      No milestone

      Relationships

      None yet

      Development

      No branches or pull requests

      Issue actions