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