예제 #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);
        }
예제 #2
0
        private void InitMotioObject()
        {
            if (_motioObject == null)
            {
                PtrClass a = ((IVdsGroupInterface)_currentView.GameLayer).GetObjectByID(ActorBindingID.Value);
                if (a == null)
                {
                    return;
                }
                _motioObject           = a as VdsActor;
                _motionObjectOriginPos = _motioObject.ActorTranslation;
                VdsVec3d   direction = new VdsVec3d(1, 0, 0);
                VdsMatrixd rMt       = new VdsMatrixd();
                VdsMatrixd.HprToMatrix(ref rMt, _motioObject.ActorRotation);
                direction = rMt.PreMult(direction);
                _motionObjectOriginDirection = direction;
            }
            if (_relevanceObject == null && ActorRelevanceID.Value != "" && (RelevanceRotation.Value || RelevancePosition.Value))
            {
                PtrClass a = ((IVdsGroupInterface)_currentView.GameLayer).GetObjectByID(ActorRelevanceID.Value);
                if (a != null)
                {
                    _relevanceObject = a as VdsActor;
                    if (RelevancePosition.Value || RelevanceRotation.Value)
                    {
                        VdsMatrixd localToWorld = new VdsMatrixd();
                        StaticMethod.ComputeCoordinateFrame(_motioObject.ParentObject as VdsActor, ref localToWorld);
                        StaticMethod.ComputeCoordinateFrame(_relevanceObject, ref _relevanceMatrixd);
                        VdsMatrixd worldToRelevance = _relevanceMatrixd.Inverse(_relevanceMatrixd);

                        _motionObjectOriginDirection = localToWorld.PreMult(_motionObjectOriginDirection);
                        _motionObjectOriginDirection = worldToRelevance.PreMult(_motionObjectOriginDirection);
                        VdsVec3d zPos = new VdsVec3d();
                        zPos = localToWorld.PreMult(zPos);
                        zPos = worldToRelevance.PreMult(zPos);
                        _motionObjectOriginDirection = _motionObjectOriginDirection - zPos;
                        _motionObjectOriginDirection.Normalize();
                        _motionObjectOriginPos = localToWorld.PreMult(_motionObjectOriginPos);
                        _motionObjectOriginPos = worldToRelevance.PreMult(_motionObjectOriginPos);
                        List <VdsVec3d> newKeyPointsList = new List <VdsVec3d>(ActorMotionKeyPoints.ValueList.Count);
                        foreach (VdsVec3d v in ActorMotionKeyPoints.ValueList)
                        {
                            VdsVec3d newV = localToWorld.PreMult(v);
                            newV = worldToRelevance.PreMult(newV);
                            newKeyPointsList.Add(newV);
                        }
                        ActorMotionKeyPoints.ValueList = newKeyPointsList;
                    }
                }
            }
        }
        private void InitMotioObject()
        {
            if (_motioObject == null)
            {
                PtrClass a = ((IVdsGroupInterface)_currentView.GameLayer).GetObjectByID(ActorBindingID.Value);
                if (a == null)
                {
                    return;
                }
                _motioObject = a as VdsActor;
                VdsVec3d   tRotate = TargetPose;
                VdsVec3d   oRotate = _motioObject.ActorRotation;
                VdsMatrixd oMt     = new VdsMatrixd();
                VdsMatrixd.HprToMatrix(ref oMt, tRotate);
                _targetPose = oMt.GetRotate();
                VdsMatrixd tMt = new VdsMatrixd();
                VdsMatrixd.HprToMatrix(ref tMt, oRotate);
                _originPose = tMt.GetRotate();
                _originPos  = _motioObject.ActorTranslation;
            }
            if (_relevanceObject == null && ActorRelevanceID.Value != "" && (RelevanceRotation.Value || RelevancePosition.Value))
            {
                PtrClass a = ((IVdsGroupInterface)_currentView.GameLayer).GetObjectByID(ActorRelevanceID.Value);
                if (a != null)
                {
                    _relevanceObject = a as VdsActor;
                    if (RelevancePosition.Value || RelevanceRotation.Value)
                    {
                        StaticMethod.ComputeCoordinateFrame(_relevanceObject, ref _relevanceMatrixd);
                        StaticMethod.ComputeCoordinateFrame(_motioObject.ParentObject as VdsActor, ref _parentLocalToWorld);
                        VdsMatrixd worldToRelevance = _relevanceMatrixd.Inverse(_relevanceMatrixd);
                        VdsMatrixd originPoseMt     = new VdsMatrixd();
                        originPoseMt.MakeRotate(_originPose);
                        originPoseMt.PreMult(_parentLocalToWorld);
                        originPoseMt.PreMult(worldToRelevance);
                        _originPose = originPoseMt.GetRotate();

                        VdsMatrixd targetPoseMt = new VdsMatrixd();
                        targetPoseMt.MakeRotate(_targetPose);
                        targetPoseMt.PreMult(_parentLocalToWorld);
                        targetPoseMt.PreMult(worldToRelevance);
                        _originPose = targetPoseMt.GetRotate();

                        _originPos = _parentLocalToWorld.PreMult(_originPos);
                        _originPos = worldToRelevance.PreMult(_originPos);
                    }
                }
            }
        }
        public override void UpdateStep(object param)
        {
            if (param == null)
            {
                return;
            }
            double?t = param as double?;

            if (t == null)
            {
                return;
            }
            _curTime = (double)t;
            if (_curTime < 0)
            {
                return;
            }
            InitMotioObject();
            VdsPlotEvent pEvent = ParentActor as VdsPlotEvent;

            if (_curTime > pEvent.EventStartTime && _curTime < pEvent.EventStartTime + pEvent.EventDurationTime)
            {
                if (_motioObject != null)
                {
                    VdsVec3d pos = _originPos;
                    double   interpolationValue = (_curTime - pEvent.EventStartTime) / pEvent.EventDurationTime;
                    VdsQuat  quat = VdsQuat.Slerp(interpolationValue, _originPose, _targetPose);
                    SetActorStatus(_motioObject, ActorStatus.Value, true);
                    VdsMatrixd rotateMt = new VdsMatrixd();
                    rotateMt.MakeRotate(quat);
                    if (_relevanceObject != null)
                    {
                        VdsMatrixd nowMt        = new VdsMatrixd();
                        VdsMatrixd localToWorld = new VdsMatrixd();
                        StaticMethod.ComputeCoordinateFrame(_relevanceObject, ref nowMt);
                        StaticMethod.ComputeCoordinateFrame(_motioObject.ParentObject as VdsActor, ref localToWorld);
                        VdsMatrixd worldToLocal = localToWorld.Inverse(localToWorld);
                        if (RelevancePosition.Value && !RelevanceRotation.Value)
                        {
                            nowMt.MakeTranslate(nowMt.GetTrans());
                            pos = nowMt.PreMult(pos);
                            pos = worldToLocal.PreMult(pos);
                            rotateMt.PreMult(nowMt);
                            rotateMt.PreMult(worldToLocal);
                        }
                        else if (!RelevancePosition.Value && RelevanceRotation.Value)
                        {
                            nowMt.MakeRotate(nowMt.GetRotate());
                            pos = _relevanceMatrixd.PreMult(pos);
                            pos = worldToLocal.PreMult(pos);
                            rotateMt.PreMult(nowMt);
                            rotateMt.PreMult(worldToLocal);
                        }
                        else if (RelevancePosition.Value && RelevanceRotation.Value)
                        {
                            rotateMt.PreMult(nowMt);
                            rotateMt.PreMult(worldToLocal);
                            pos = nowMt.PreMult(pos);
                            pos = worldToLocal.PreMult(pos);
                        }
                    }
                    VdsVec3d zr = new VdsVec3d();
                    VdsMatrixd.MatrixToHpr(ref zr, rotateMt);
                    VdsVec3d rotation = new VdsVec3d(0, 0, zr.Z);
                    _motioObject.ActorRotation    = rotation;
                    _motioObject.ActorTranslation = pos;
                }
            }
            else if (_motioObject != null)
            {
                SetActorStatus(_motioObject, "DefaultStatus", false);
            }
        }
예제 #5
0
        public static void MatrixToHpr(ref VdsVec3d hpr, VdsMatrixd rotation)
        {
            VdsMatrixd   mat          = new VdsMatrixd();
            VdsVec3d     col1         = new VdsVec3d(rotation.Mat[0, 0], rotation.Mat[0, 1], rotation.Mat[0, 2]);
            double       s            = col1.Length();
            const double magicEpsilon = 0.00001;

            if (s <= magicEpsilon)
            {
                hpr.X = 0.0;
                hpr.Y = 0.0;
                hpr.Z = 0.0;
                return;
            }
            double oneOverS = 1.0f / s;

            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 3; ++j)
                {
                    mat.Mat[i, j] = rotation.Mat[i, j] * oneOverS;
                }
            }
            double sinPitch = StaticMethod.ClampUnity(mat.Mat[1, 2]);
            double pitch    = Math.Asin(sinPitch);

            hpr.Y = StaticMethod.RadiansToDegrees(pitch);
            double cp = Math.Cos(pitch);

            if (cp > -magicEpsilon && cp < magicEpsilon)
            {
                double cr = StaticMethod.ClampUnity(-mat.Mat[2, 1]);
                double sr = StaticMethod.ClampUnity(mat.Mat[0, 1]);
                hpr.X = 0.0f;
                hpr.Z = StaticMethod.RadiansToDegrees(Math.Atan2(sr, cr));
            }
            else
            {
                double oneOverCp = 1.0 / cp;
                double sr        = StaticMethod.ClampUnity(-mat.Mat[0, 2] * oneOverCp);
                double cr        = StaticMethod.ClampUnity(mat.Mat[2, 2] * oneOverCp);
                double sh        = StaticMethod.ClampUnity(-mat.Mat[1, 0] * oneOverCp);
                double ch        = StaticMethod.ClampUnity(mat.Mat[1, 1] * oneOverCp);
                if ((StaticMethod.Equivalent(sh, 0.0, magicEpsilon) && StaticMethod.Equivalent(ch, 0.0, magicEpsilon)) ||
                    (StaticMethod.Equivalent(sr, 0.0, magicEpsilon) && StaticMethod.Equivalent(cr, 0.0, magicEpsilon)))
                {
                    cr    = StaticMethod.ClampUnity(-mat.Mat[2, 1]);
                    sr    = StaticMethod.ClampUnity(mat.Mat[0, 1]);;
                    hpr.X = 0.0f;
                }
                else
                {
                    hpr.X = StaticMethod.RadiansToDegrees(Math.Atan2(sh, ch));
                }
                hpr.Z = StaticMethod.RadiansToDegrees(Math.Atan2(sr, cr));
            }
            double tmp = hpr.X;

            hpr.X = hpr.Y;
            hpr.Y = hpr.Z;
            hpr.Z = tmp;
        }
예제 #6
0
        private bool Invert_4x4(VdsMatrixd mat)
        {
            if (mat == this)
            {
                VdsMatrixd tm = new VdsMatrixd(mat);
                return(Invert_4x4(tm));
            }
            int[]  indxc = new int[4];
            int[]  indxr = new int[4];
            int[]  ipiv = new int[4];
            int    i, j, k, l, ll;
            int    icol = 0;
            int    irow = 0;
            double pivinv, dum, big;

            Mat = mat.Mat;
            for (j = 0; j < 4; j++)
            {
                ipiv[j] = 0;
            }
            for (i = 0; i < 4; i++)
            {
                big = 0.0;
                for (j = 0; j < 4; j++)
                {
                    if (ipiv[j] != 1)
                    {
                        for (k = 0; k < 4; k++)
                        {
                            if (ipiv[k] == 0)
                            {
                                if (Math.Abs(Mat[j, k]) >= big)
                                {
                                    big  = Math.Abs(Mat[j, k]);
                                    irow = j;
                                    icol = k;
                                }
                            }
                            else if (ipiv[k] > 1)
                            {
                                return(false);
                            }
                        }
                    }
                }
                ++(ipiv[icol]);
                if (irow != icol)
                {
                    for (l = 0; l < 4; l++)
                    {
                        StaticMethod.Swap(ref Mat[irow, l], ref Mat[icol, l]);
                    }
                }
                indxr[i] = irow;
                indxc[i] = icol;
                if (Mat[icol, icol] == 0)
                {
                    return(false);
                }
                pivinv          = 1.0 / Mat[icol, icol];
                Mat[icol, icol] = 1;
                for (l = 0; l < 4; l++)
                {
                    Mat[icol, l] *= pivinv;
                }
                for (ll = 0; ll < 4; ll++)
                {
                    if (ll != icol)
                    {
                        dum           = Mat[ll, icol];
                        Mat[ll, icol] = 0;
                        for (l = 0; l < 4; l++)
                        {
                            Mat[ll, l] -= Mat[icol, l] * dum;
                        }
                    }
                }
            }
            for (int lx = 4; lx > 0; --lx)
            {
                if (indxr[lx - 1] != indxc[lx - 1])
                {
                    for (k = 0; k < 4; k++)
                    {
                        StaticMethod.Swap(ref Mat[k, indxr[lx - 1]], ref Mat[k, indxc[lx - 1]]);
                    }
                }
            }
            return(true);
        }
예제 #7
0
        public override void UpdateStep(object param)
        {
            if (param == null)
            {
                return;
            }
            double?t = param as double?;

            if (t == null)
            {
                return;
            }
            _curTime = (double)t;
            if (_curTime < 0)
            {
                return;
            }
            InitMotioObject();
            VdsPlotEvent pEvent = ParentActor as VdsPlotEvent;

            if (_curTime > pEvent.EventStartTime && _curTime < pEvent.EventStartTime + pEvent.EventDurationTime)
            {
                if (_motioObject != null)
                {
                    VdsVec3d pos       = new VdsVec3d();
                    VdsVec3d direction = new VdsVec3d();
                    GetTranslationAndRotation(_motioObject, _curTime - pEvent.EventStartTime, out pos, out direction);
                    SetActorStatus(_motioObject, ActorStatus.Value, true);
                    if (_relevanceObject != null)
                    {
                        VdsMatrixd nowMt        = new VdsMatrixd();
                        VdsMatrixd localToWorld = new VdsMatrixd();
                        StaticMethod.ComputeCoordinateFrame(_relevanceObject, ref nowMt);
                        StaticMethod.ComputeCoordinateFrame(_motioObject.ParentObject as VdsActor, ref localToWorld);
                        VdsMatrixd worldToLocal = localToWorld.Inverse(localToWorld);
                        if (RelevancePosition.Value && !RelevanceRotation.Value)
                        {
                            nowMt.MakeTranslate(nowMt.GetTrans());
                            pos = nowMt.PreMult(pos);
                            pos = worldToLocal.PreMult(pos);
                        }
                        else if (!RelevancePosition.Value && RelevanceRotation.Value)
                        {
                            nowMt.MakeRotate(nowMt.GetRotate());
                            direction = nowMt.PreMult(direction);
                            direction = worldToLocal.PreMult(direction);
                            VdsVec3d zPos = new VdsVec3d();
                            zPos      = nowMt.PreMult(zPos);
                            zPos      = worldToLocal.PreMult(zPos);
                            direction = direction - zPos;

                            pos = _relevanceMatrixd.PreMult(pos);
                            pos = worldToLocal.PreMult(pos);
                        }
                        else if (RelevancePosition.Value && RelevanceRotation.Value)
                        {
                            direction = nowMt.PreMult(direction);
                            direction = worldToLocal.PreMult(direction);
                            VdsVec3d zPos = new VdsVec3d();
                            zPos      = nowMt.PreMult(zPos);
                            zPos      = worldToLocal.PreMult(zPos);
                            direction = direction - zPos;

                            pos = nowMt.PreMult(pos);
                            pos = worldToLocal.PreMult(pos);
                        }
                    }
                    direction.Normalize();
                    VdsVec3d   zr  = new VdsVec3d();
                    VdsMatrixd rMt = new VdsMatrixd();
                    rMt.MakeRotate(new VdsVec3d(1, 0, 0), direction);
                    VdsMatrixd.MatrixToHpr(ref zr, rMt);
                    VdsVec3d rotation = new VdsVec3d(0, 0, zr.Z);
                    _motioObject.ActorRotation    = rotation;
                    _motioObject.ActorTranslation = pos;
                }
            }
            else if (_motioObject != null)
            {
                SetActorStatus(_motioObject, "DefaultStatus", false);
            }
        }