public VdsMatrixd Inverse(VdsMatrixd matrix) { VdsMatrixd m = new VdsMatrixd(); m.Invert(matrix); return(m); }
public void Mult(VdsMatrixd lhs, VdsMatrixd rhs) { if (lhs == this) { PostMult(rhs); return; } if (rhs == this) { PreMult(lhs); return; } _mat[0, 0] = InnerProduct(lhs, rhs, 0, 0); _mat[0, 1] = InnerProduct(lhs, rhs, 0, 1); _mat[0, 2] = InnerProduct(lhs, rhs, 0, 2); _mat[0, 3] = InnerProduct(lhs, rhs, 0, 3); _mat[1, 0] = InnerProduct(lhs, rhs, 1, 0); _mat[1, 1] = InnerProduct(lhs, rhs, 1, 1); _mat[1, 2] = InnerProduct(lhs, rhs, 1, 2); _mat[1, 3] = InnerProduct(lhs, rhs, 1, 3); _mat[2, 0] = InnerProduct(lhs, rhs, 2, 0); _mat[2, 1] = InnerProduct(lhs, rhs, 2, 1); _mat[2, 2] = InnerProduct(lhs, rhs, 2, 2); _mat[2, 3] = InnerProduct(lhs, rhs, 2, 3); _mat[3, 0] = InnerProduct(lhs, rhs, 3, 0); _mat[3, 1] = InnerProduct(lhs, rhs, 3, 1); _mat[3, 2] = InnerProduct(lhs, rhs, 3, 2); _mat[3, 3] = InnerProduct(lhs, rhs, 3, 3); }
private double InnerProduct(VdsMatrixd a, VdsMatrixd b, int r, int c) { return((a._mat[r, 0] * (b)._mat[0, c]) + (a._mat[r, 1] * (b)._mat[1, c]) + (a._mat[r, 2] * (b)._mat[2, c]) + (a._mat[r, 3] * (b)._mat[3, c])); }
public VdsMatrixd(VdsMatrixd mat) { for (int r = 0; r < 4; ++r) { for (int c = 0; c < 4; ++c) { _mat[r, c] = mat._mat[r, c]; } } }
public void PostMult(VdsMatrixd other) { double[] t = new double[4]; for (int row = 0; row < 4; ++row) { t[0] = InnerProduct(this, other, row, 0); t[1] = InnerProduct(this, other, row, 1); t[2] = InnerProduct(this, other, row, 2); t[3] = InnerProduct(this, other, row, 3); SetRow(row, t[0], t[1], t[2], t[3]); } }
public override void End() { if (_currentView != null && _motioObject != null) { _motioObject = null; } _relevanceObject = null; _relevanceMatrixd = null; _behaviourIsWorking = false; _currentView = null; base.End(); }
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; } } } }
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); }
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 void PreMult(VdsMatrixd other) { double[] t = new double[4]; for (int col = 0; col < 4; ++col) { t[0] = InnerProduct(other, this, 0, col); t[1] = InnerProduct(other, this, 1, col); t[2] = InnerProduct(other, this, 2, col); t[3] = InnerProduct(other, this, 3, col); _mat[0, col] = t[0]; _mat[1, col] = t[1]; _mat[2, col] = t[2]; _mat[3, col] = t[3]; } }
public override void End() { if (_currentView != null && _motioObject != null) { _motioObject = null; } _relevanceObject = null; _relevanceMatrixd = null; _parentLocalToWorld = null; _behaviourIsWorking = false; _originPose = null; _originPos = null; _targetPose = null; _currentView = null; base.End(); }
public static void ComputeCoordinateFrame(VdsActor actor, ref VdsMatrixd localToWorld) { VdsActor curActor = actor; localToWorld.MakeIdentity(); while (curActor != null) { VdsMatrixd transMat = new VdsMatrixd(); transMat.MakeTranslate(curActor.ActorTranslation); VdsMatrixd rotateMat = new VdsMatrixd(); VdsMatrixd.HprToMatrix(ref rotateMat, curActor.ActorRotation); rotateMat.PostMult(transMat); localToWorld.PreMult(rotateMat); if (curActor.ParentObject != null && curActor.ParentObject is VdsActor) { curActor = curActor.ParentObject as VdsActor; } else { curActor = null; } } }
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); } }
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); } }
public bool Invert(VdsMatrixd rhs) { bool is_4x3 = (rhs._mat[0, 3] == 0.0 && rhs._mat[1, 3] == 0.0 && rhs._mat[2, 3] == 0.0 && rhs._mat[3, 3] == 1.0); return(is_4x3 ? Invert_4x3(rhs) : Invert_4x4(rhs)); }
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); }
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; }
private bool Invert_4x3(VdsMatrixd mat) { if (mat == this) { VdsMatrixd tm = new VdsMatrixd(mat); return(Invert_4x3(tm)); } double r00, r01, r02, r10, r11, r12, r20, r21, r22; r00 = mat._mat[0, 0]; r01 = mat._mat[0, 1]; r02 = mat._mat[0, 2]; r10 = mat._mat[1, 0]; r11 = mat._mat[1, 1]; r12 = mat._mat[1, 2]; r20 = mat._mat[2, 0]; r21 = mat._mat[2, 1]; r22 = mat._mat[2, 2]; _mat[0, 0] = r11 * r22 - r12 * r21; _mat[0, 1] = r02 * r21 - r01 * r22; _mat[0, 2] = r01 * r12 - r02 * r11; double oneOverDet = 1.0 / (r00 * _mat[0, 0] + r10 * _mat[0, 1] + r20 * _mat[0, 2]); r00 *= oneOverDet; r10 *= oneOverDet; r20 *= oneOverDet; _mat[0, 0] *= oneOverDet; _mat[0, 1] *= oneOverDet; _mat[0, 2] *= oneOverDet; _mat[0, 3] = 0.0; _mat[1, 0] = r12 * r20 - r10 * r22; _mat[1, 1] = r00 * r22 - r02 * r20; _mat[1, 2] = r02 * r10 - r00 * r12; _mat[1, 3] = 0.0; _mat[2, 0] = r10 * r21 - r11 * r20; _mat[2, 1] = r01 * r20 - r00 * r21; _mat[2, 2] = r00 * r11 - r01 * r10; _mat[2, 3] = 0.0; _mat[3, 3] = 1.0; r22 = mat._mat[3, 3]; if ((r22 - 1.0) * (r22 - 1.0) > 0.0000001) { VdsMatrixd tpInv = new VdsMatrixd(); _mat[3, 0] = _mat[3, 1] = _mat[3, 2] = 0.0; r10 = mat._mat[0, 3]; r11 = mat._mat[1, 3]; r12 = mat._mat[2, 3]; r00 = _mat[0, 0] * r10 + _mat[0, 1] * r11 + _mat[0, 2] * r12; r01 = _mat[1, 0] * r10 + _mat[1, 1] * r11 + _mat[1, 2] * r12; r02 = _mat[2, 0] * r10 + _mat[2, 1] * r11 + _mat[2, 2] * r12; r10 = mat._mat[3, 0]; r11 = mat._mat[3, 1]; r12 = mat._mat[3, 2]; oneOverDet = 1.0 / (r22 - (r10 * r00 + r11 * r01 + r12 * r02)); r10 *= oneOverDet; r11 *= oneOverDet; r12 *= oneOverDet; tpInv._mat[0, 0] = r10 * r00 + 1.0; tpInv._mat[0, 1] = r11 * r00; tpInv._mat[0, 2] = r12 * r00; tpInv._mat[0, 3] = -r00 * oneOverDet; tpInv._mat[1, 0] = r10 * r01; tpInv._mat[1, 1] = r11 * r01 + 1.0; tpInv._mat[1, 2] = r12 * r01; tpInv._mat[1, 3] = -r01 * oneOverDet; tpInv._mat[2, 0] = r10 * r02; tpInv._mat[2, 1] = r11 * r02; tpInv._mat[2, 2] = r12 * r02 + 1.0; tpInv._mat[2, 3] = -r02 * oneOverDet; tpInv._mat[3, 0] = -r10; tpInv._mat[3, 1] = -r11; tpInv._mat[3, 2] = -r12; tpInv._mat[3, 3] = oneOverDet; PreMult(tpInv); } else { r10 = mat._mat[3, 0]; r11 = mat._mat[3, 1]; r12 = mat._mat[3, 2]; _mat[3, 0] = -(r10 * _mat[0, 0] + r11 * _mat[1, 0] + r12 * _mat[2, 0]); _mat[3, 1] = -(r10 * _mat[0, 1] + r11 * _mat[1, 1] + r12 * _mat[2, 1]); _mat[3, 2] = -(r10 * _mat[0, 2] + r11 * _mat[1, 2] + r12 * _mat[2, 2]); } return(true); }