WebCAD/src/Common/Matrix4Utils.ts

import { Matrix4, Quaternion, Vector2, Vector3 } from 'three';
import { CoordinateSystem } from '../Geometry/CoordinateSystem';
import { equaln, isParallelTo } from '../Geometry/GeUtils';

/**
 * 设置矩阵的某列的向量
 * @param {Matrix4} mtx 矩阵
 * @param {number} col 列索引,0x 1y 2z 3org
 * @param {Vector3} v 向量或点
 */
export function SetMtxVector(mtx: Matrix4, col: number, v: Vector3)
{
    let index = col * 4;
    mtx.elements[index] = v.x;
    mtx.elements[index + 1] = v.y;
    mtx.elements[index + 2] = v.z;
}

/**
 * 返回矩阵，该坐标系将坐标系与原点的坐标系映射为坐标系，
 * 并将坐标系与X轴坐标系，
 * Y轴坐标轴以及Z轴坐标系统之间的坐标系统坐标系统的原点坐标系和原点坐标系统坐标轴的坐标系分别设置为XAxis，YAxis和ZAxis
 * @returns {Matrix4} 返回新的矩阵
 */
export function matrixAlignCoordSys(matrixFrom: Matrix4, matrixTo: Matrix4): Matrix4
{
    return new Matrix4().getInverse(matrixTo).multiply(matrixFrom);
}

/**
 * 判断2个矩形共面
 * @param {Matrix4} matrixFrom
 * @param {Matrix4} matrixTo
 * @returns {boolean} 2个矩阵共面
 */
export function matrixIsCoplane(matrixFrom: Matrix4, matrixTo: Matrix4, fuzz = 1e-5): boolean
{
    let nor1 = new Vector3().setFromMatrixColumn(matrixFrom, 2);
    let nor2 = new Vector3().setFromMatrixColumn(matrixTo, 2);

    //法线共面
    if (!isParallelTo(nor1, nor2))
        return false;

    //高共面
    let pt = new Vector3().setFromMatrixPosition(matrixTo);
    //变换到自身对象坐标系.
    pt.applyMatrix4(new Matrix4().getInverse(matrixFrom));

    return equaln(pt.z, 0, fuzz);
}

//构造缩放矩阵 等比例
export function matrixScale(scale: number, center?: Vector3)
{
    let scaleMtx = new Matrix4().makeScale(scale, scale, scale);
    if (center) scaleMtx.setPosition(center.clone().multiplyScalar(1 - scale));
    return scaleMtx;
}

//缩放矩阵 不等比例
export function MakeScaleMatrix(scaleX: number, scaleY: number, scaleZ: number, center?: Vector3)
{
    let scaleMtx = new Matrix4().makeScale(scaleX, scaleY, scaleZ);
    if (center) scaleMtx.setPosition(center.clone().applyMatrix4(scaleMtx).sub(center).negate());
    return scaleMtx;
}

/**
 * 设置旋转矩阵,不改变矩阵的基点
 */
export function setRotationOnAxis(mtx: Matrix4, axis: Vector3, ro: number)
{
    let pos = new Vector3().setFromMatrixPosition(mtx);
    mtx.makeRotationAxis(axis, ro);
    mtx.setPosition(pos);
    return mtx;
}

/**
 * 修正镜像后矩阵
 */
export function reviseMirrorMatrix(mtx: Matrix4, index = 1): Matrix4
{
    let cs = new CoordinateSystem().applyMatrix4(mtx);
    if (index === 0)
        cs.XAxis.negate();
    else if (index === 1)
        cs.YAxis.negate();
    else
        cs.ZAxis.negate();

    return cs.getMatrix4(mtx);
}

let cacheVec: Vector3;
export function Vector2ApplyMatrix4(mtx: Matrix4, vec: Vector2)
{
    if (!cacheVec) cacheVec = new Vector3();

    cacheVec.x = vec.x;
    cacheVec.y = vec.y;
    cacheVec.z = 0;

    cacheVec.applyMatrix4(mtx);

    vec.x = cacheVec.x;
    vec.y = cacheVec.y;
}

export function MakeMirrorMtx(planeNormal: Vector3, pos?: Vector3)
{
    let mirrorMtx = new Matrix4();
    let xAxis = new Vector3(1 - 2 * planeNormal.x ** 2, -2 * planeNormal.x * planeNormal.y, -2 * planeNormal.x * planeNormal.z);
    let yAxis = new Vector3(-2 * planeNormal.x * planeNormal.y, 1 - 2 * planeNormal.y ** 2, -2 * planeNormal.y * planeNormal.z);
    let zAxis = new Vector3(-2 * planeNormal.x * planeNormal.z, -2 * planeNormal.y * planeNormal.z, 1 - 2 * planeNormal.z ** 2);
    mirrorMtx.makeBasis(xAxis, yAxis, zAxis);

    if (pos)
        mirrorMtx.setPosition(pos.clone().applyMatrix4(mirrorMtx).sub(pos).negate());

    return mirrorMtx;
}

/**
 * 对向量进行方向变换  (如果是pos 请使用pos.applyMatrix4)
 * @param vec 向量
 * @param m 矩阵
 * @returns vec
 */
export function TransformVector<T extends { x: number, y: number, z: number; }>(vec: T, m: Matrix4): T
{
    let { x, y, z } = vec;
    let e = m.elements;
    vec.x = e[0] * x + e[4] * y + e[8] * z;
    vec.y = e[1] * x + e[5] * y + e[9] * z;
    vec.z = e[2] * x + e[6] * y + e[10] * z;
    return vec;
}

export function MakeRotateMatrix4(cen: Vector3, axis: Vector3, angle: number, roMtx: Matrix4 = new Matrix4)
{
    roMtx.makeRotationAxis(axis, angle);
    roMtx.setPosition(cen.clone().applyMatrix4(roMtx).negate().add(cen));
    return roMtx;
}

/**
 * 把变换矩阵展平成2d矩阵,避免出现三维坐标.
 */
export function MatrixPlanarizere(mtx: Matrix4, z0 = true)
{
    mtx.elements[2] = 0;
    mtx.elements[6] = 0;
    mtx.elements[8] = 0;
    mtx.elements[9] = 0;
    mtx.elements[10] = Math.sign(mtx.elements[10]);

    if (z0)
        mtx.elements[14] = 0;

    return mtx;
}

let p = new Vector3;
let s = new Vector3;
let q = new Quaternion;
//归一化矩阵 避免轴不是非标准向量
export function NormalMatrix(mtx: Matrix4)
{
    mtx.decompose(p, q, s);
    s.set(1, 1, 1);
    mtx.compose(p, q, s);
    return mtx;
}

//四舍五入
export function RoundMatrix(mtx: Matrix4, fuzz = 1e-6)
{
    let el = mtx.elements;
    for (let i = 0; i < 16; i++)
    {
        let re = Math.round(el[i]);
        if (equaln(re, el[i], fuzz))
            el[i] = re;
    }
    return mtx;
}

export const tempMatrix1 = new Matrix4;

export const ZMirrorMatrix = MakeMirrorMtx(new Vector3(0, 0, 1));