public void MakeRotate(VdsQuat q)
{
MakeIdentity();
double length2 = q.Length2();
if (Math.Abs(length2) <= Double.MinValue)
{
_mat[0, 0] = 0.0; _mat[1, 0] = 0.0; _mat[2, 0] = 0.0;
_mat[0, 1] = 0.0; _mat[1, 1] = 0.0; _mat[2, 1] = 0.0;
_mat[0, 2] = 0.0; _mat[1, 2] = 0.0; _mat[2, 2] = 0.0;
}
else
{
double rlength2;
if (length2 != 1.0)
{
rlength2 = 2.0 / length2;
}
else
{
rlength2 = 2.0;
}
double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
x2 = rlength2 * q.QuatValue[0];
y2 = rlength2 * q.QuatValue[1];
z2 = rlength2 * q.QuatValue[2];
xx = q.QuatValue[0] * x2;
xy = q.QuatValue[0] * y2;
xz = q.QuatValue[0] * z2;
yy = q.QuatValue[1] * y2;
yz = q.QuatValue[1] * z2;
zz = q.QuatValue[2] * z2;
wx = q.QuatValue[3] * x2;
wy = q.QuatValue[3] * y2;
wz = q.QuatValue[3] * z2;
_mat[0, 0] = 1.0 - (yy + zz);
_mat[1, 0] = xy - wz;
_mat[2, 0] = xz + wy;
_mat[0, 1] = xy + wz;
_mat[1, 1] = 1.0 - (xx + zz);
_mat[2, 1] = yz - wx;
_mat[0, 2] = xz - wy;
_mat[1, 2] = yz + wx;
_mat[2, 2] = 1.0 - (xx + yy);
}
}
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 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 void MakeRotate(double angle1, VdsVec3d axis1,
double angle2, VdsVec3d axis2,
double angle3, VdsVec3d axis3)
{
VdsQuat q1 = new VdsQuat();
q1.MakeRotate(angle1, axis1);
VdsQuat q2 = new VdsQuat();
q2.MakeRotate(angle2, axis2);
VdsQuat q3 = new VdsQuat();
q3.MakeRotate(angle3, axis3);
VdsQuat q = q1 * q2 * q3;
SetQuat(q);
}
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);
}
}