Line data Source code
1 : import type { Vector3Tuple as Vec3 } from '@comfyorg/shared-frontend-utils/geometry'
2 :
3 : interface CameraFromMatricesResult {
4 : position: Vec3
5 : target: Vec3
6 : fovYDegrees: number
7 : }
8 :
9 0 : export function computeCameraFromMatrices(
10 : extrinsics: readonly (readonly number[])[],
11 : intrinsics: readonly (readonly number[])[]
12 : ): CameraFromMatricesResult {
13 0 : assertMatrixShape(extrinsics, 4, 4, 'extrinsics')
14 0 : assertMatrixShape(intrinsics, 3, 3, 'intrinsics')
15 :
16 0 : const r00 = extrinsics[0][0]
17 0 : const r01 = extrinsics[0][1]
18 0 : const r02 = extrinsics[0][2]
19 0 : const r10 = extrinsics[1][0]
20 0 : const r11 = extrinsics[1][1]
21 0 : const r12 = extrinsics[1][2]
22 0 : const r20 = extrinsics[2][0]
23 0 : const r21 = extrinsics[2][1]
24 0 : const r22 = extrinsics[2][2]
25 :
26 0 : const tx = extrinsics[0][3]
27 0 : const ty = extrinsics[1][3]
28 0 : const tz = extrinsics[2][3]
29 :
30 0 : const posX = -(r00 * tx + r10 * ty + r20 * tz)
31 0 : const posY = -(r01 * tx + r11 * ty + r21 * tz)
32 0 : const posZ = -(r02 * tx + r12 * ty + r22 * tz)
33 :
34 0 : const targetX = posX + r20
35 0 : const targetY = posY + r21
36 0 : const targetZ = posZ + r22
37 :
38 0 : const fy = intrinsics[1][1]
39 0 : const cy = intrinsics[1][2]
40 0 : if (!Number.isFinite(fy) || fy === 0) {
41 0 : throw new Error(
42 : `intrinsics[1][1] (fy) must be a non-zero finite number, got ${fy}`
43 : )
44 : }
45 : const fovYRad = 2 * Math.atan(cy / fy)
46 0 : const fovYDegrees = (fovYRad * 180) / Math.PI
47 :
48 0 : return {
49 : position: [posX, -posY, -posZ],
50 : target: [targetX, -targetY, -targetZ],
51 : fovYDegrees
52 : }
53 : }
54 :
55 0 : function assertMatrixShape(
56 : matrix: readonly (readonly number[])[],
57 : rows: number,
58 : cols: number,
59 : name: string
60 : ): void {
61 0 : if (matrix.length !== rows) {
62 0 : throw new Error(
63 : `${name} must be ${rows}x${cols}, got ${matrix.length} rows`
64 : )
65 : }
66 0 : for (let i = 0; i < rows; i++) {
67 0 : if (matrix[i].length !== cols) {
68 0 : throw new Error(
69 : `${name} row ${i} must have ${cols} columns, got ${matrix[i].length}`
70 : )
71 : }
72 : }
73 : }
|