コード例 #1
0
ファイル: Mat4f.cs プロジェクト: abdalmoez/amathlib
        public void Rotate(float angle, Vec3f v)
        {
            var a = angle;
            var c = (float)Math.Cos(a);
            var s = (float)Math.Sin(a);

            Vec3f axis = v.CreateNormalizedVector();
            Vec3f temp = (1 - c) * axis;

            Mat4f Rotate = new Mat4f();

            Rotate[0][0] = c + temp[0] * axis[0];
            Rotate[0][1] = temp[0] * axis[1] + s * axis[2];
            Rotate[0][2] = temp[0] * axis[2] - s * axis[1];

            Rotate[1][0] = temp[1] * axis[0] - s * axis[2];
            Rotate[1][1] = c + temp[1] * axis[1];
            Rotate[1][2] = temp[1] * axis[2] + s * axis[0];

            Rotate[2][0] = temp[2] * axis[0] + s * axis[1];
            Rotate[2][1] = temp[2] * axis[1] - s * axis[0];
            Rotate[2][2] = c + temp[2] * axis[2];

            Mat4f Result = new Mat4f();

            Result[0] = this[0] * Rotate[0][0] + this[1] * Rotate[0][1] + this[2] * Rotate[0][2];
            Result[1] = this[0] * Rotate[1][0] + this[1] * Rotate[1][1] + this[2] * Rotate[1][2];
            Result[2] = this[0] * Rotate[2][0] + this[1] * Rotate[2][1] + this[2] * Rotate[2][2];
            Result[3] = this[3];
            for (int i = 0; i < Size; i++)
            {
                for (int j = 0; j < Size; j++)
                {
                    Result[i, j] = (float)Result[i, j];
                }
            }
            Set(Result);
        }