* Create a 4x4 transformation matrix from position and Euler angles
(x, y, z, rx, ry, rz)
| 481 | * Create a 4x4 transformation matrix from position and Euler angles |
| 482 | */ |
| 483 | static createPoseMatrix(x, y, z, rx, ry, rz) { |
| 484 | // Rotation matrices |
| 485 | const cosX = Math.cos(rx), sinX = Math.sin(rx); |
| 486 | const cosY = Math.cos(ry), sinY = Math.sin(ry); |
| 487 | const cosZ = Math.cos(rz), sinZ = Math.sin(rz); |
| 488 | |
| 489 | // Combined rotation matrix (ZYX order) |
| 490 | const r00 = cosY * cosZ; |
| 491 | const r01 = cosX * sinZ + sinX * sinY * cosZ; |
| 492 | const r02 = sinX * sinZ - cosX * sinY * cosZ; |
| 493 | |
| 494 | const r10 = -cosY * sinZ; |
| 495 | const r11 = cosX * cosZ - sinX * sinY * sinZ; |
| 496 | const r12 = sinX * cosZ + cosX * sinY * sinZ; |
| 497 | |
| 498 | const r20 = sinY; |
| 499 | const r21 = -sinX * cosY; |
| 500 | const r22 = cosX * cosY; |
| 501 | |
| 502 | return [ |
| 503 | [r00, r01, r02, x], |
| 504 | [r10, r11, r12, y], |
| 505 | [r20, r21, r22, z], |
| 506 | [0, 0, 0, 1] |
| 507 | ]; |
| 508 | } |
| 509 | |
| 510 | /** |
| 511 | * Create an identity 4x4 matrix |