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;
_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);
}
}
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_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 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);
}
}
