Beispiel #1
0
        public static void HprToMatrix(ref VdsMatrixd rotation, VdsVec3d hpr)
        {
            double tmp = hpr.X;

            hpr.X = hpr.Z;
            hpr.Z = hpr.Y;
            hpr.Y = tmp;
            double ch, sh, cp, sp, cr, sr, srsp, crsp, srcp;
            double magicEpsilon = 0.00001;

            if (StaticMethod.Equivalent(hpr.X, 0.0, magicEpsilon))
            {
                ch = 1.0;
                sh = 0.0;
            }
            else
            {
                sh = Math.Sin(StaticMethod.DegreesToRadians(hpr.X));
                ch = Math.Cos(StaticMethod.DegreesToRadians(hpr.X));
            }
            if (StaticMethod.Equivalent(hpr.Y, 0.0, magicEpsilon))
            {
                cp = 1.0;
                sp = 0.0;
            }
            else
            {
                sp = Math.Sin(StaticMethod.DegreesToRadians(hpr.Y));
                cp = Math.Cos(StaticMethod.DegreesToRadians(hpr.Y));
            }
            if (StaticMethod.Equivalent(hpr.Z, 0.0, magicEpsilon))
            {
                cr   = 1.0;
                sr   = 0.0;
                srsp = 0.0;
                srcp = 0.0;
                crsp = sp;
            }
            else
            {
                sr   = Math.Sin(StaticMethod.DegreesToRadians(hpr.Z));
                cr   = Math.Cos(StaticMethod.DegreesToRadians(hpr.Z));
                srsp = sr * sp;
                crsp = cr * sp;
                srcp = sr * cp;
            }
            rotation.SetMatrixd(ch * cr - sh * srsp, cr * sh + srsp * ch, -srcp, 0.0,
                                -sh * cp, ch * cp, sp, 0.0,
                                sr * ch + sh * crsp, sr * sh - crsp * ch, cr * cp, 0.0,
                                0.0, 0.0, 0.0, 1.0);
        }