空间向量绕任一向量旋转计算

假定向量P绕单位向量A旋转角度θ，得到新的向量P'，则：

P'=P * cosθ + (A×P)sinθ +A(A·P)(1 - cosθ)

其中A为单位向量，旋转角度θ为逆时针方向旋转的角度。

假定向量P的坐标为（px，py，pz），向量A的坐标为（ax，by，cz）

且：

A×P=（ay * pz- az * py, ax * pz- az * px , ax * py- ay * px）

A·P = ax * px + ay * py + az * pz

则：

Px’= px * cosθ+( ay * pz- az * py)sinθ + ax (ax * px + ay * py + az * pz)(1 - cosθ)

Py’= py * cosθ+( ax * pz- az * px)sinθ + ay (ax * px + ay * py + az * pz)(1 - cosθ)

Pz’= pz * cosθ+( ax * py- ay * px)sinθ + az (ax * px + ay * py + az * pz)(1 - cosθ)

 

 

inline FVector FVector::RotateAngleAxis( const float AngleDeg, const FVector& Axis ) const
{
    const float S    = FMath::Sin(AngleDeg * PI / 180.f);
    const float C    = FMath::Cos(AngleDeg * PI / 180.f);

    const float XX    = Axis.X * Axis.X;
    const float YY    = Axis.Y * Axis.Y;
    const float ZZ    = Axis.Z * Axis.Z;

    const float XY    = Axis.X * Axis.Y;
    const float YZ    = Axis.Y * Axis.Z;
    const float ZX    = Axis.Z * Axis.X;

    const float XS    = Axis.X * S;
    const float YS    = Axis.Y * S;
    const float ZS    = Axis.Z * S;

    const float OMC    = 1.f - C;

    return FVector(
        (OMC * XX + C ) * X + (OMC * XY - ZS) * Y + (OMC * ZX + YS) * Z,
        (OMC * XY + ZS) * X + (OMC * YY + C ) * Y + (OMC * YZ - XS) * Z,
        (OMC * ZX - YS) * X + (OMC * YZ + XS) * Y + (OMC * ZZ + C ) * Z
        );
}