public IList <SequenceData> OutputSequence(IList <MotionObjectInfo> selected, IList <ProcParam <MotionProcEnv> > args, IEnumerable <ReadOnlyMotionFrame> frames, ProgressInformation progressInfo)
{
MotionObjectSingleSelectParameter main = args[0] as MotionObjectSingleSelectParameter;
SingleSelectParameter unit = args[1] as SingleSelectParameter;
bool useRatio = unit.Value == 0;
MotionObjectInfo firstAxis = main.Value;
MotionObjectInfo secondAxis = selected.Where(info => info.IsTypeOf(typeof(LineObject)) && info != firstAxis).First();
IList <MotionObjectInfo> pointInfoList = selected.Where(info => info.IsTypeOf(typeof(PointObject))).ToList();
List <SequenceData> ret = new List <SequenceData>();
foreach (MotionObjectInfo pointInfo in pointInfoList)
{
TimeSeriesValues values = new TimeSeriesValues("axis1", "axis2", "axis3");
foreach (var frame in frames)
{
PointObject point = frame[pointInfo] as PointObject;
LineObject line1 = frame[firstAxis] as LineObject;
LineObject line2 = frame[secondAxis] as LineObject;
decimal?[] relValues = null;
if (point != null && line1 != null && line2 != null)
{
Vector3 anchor = line1.Position;
Vector3 pointRelPos = point.Position - anchor;
Vector3 axis1 = line1.Edge;
Vector3 axis2 = line2.Edge;
Vector3 axis1norm = Vector3.Normalize(axis1);
Vector3 axis2norm = Vector3.Normalize(axis2);
Vector3 axis3 = Vector3.Cross(axis1norm, axis2norm) * (float)Math.Sqrt(axis1.Length() * axis2.Length());
if (!useRatio)
{
axis1 = Vector3.Normalize(axis1);
axis2 = Vector3.Normalize(axis2);
axis3 = Vector3.Normalize(axis3);
}
float[,] mat = new float[, ] {
{ axis1.X, axis2.X, axis3.X }, { axis1.Y, axis2.Y, axis3.Y }, { axis1.Z, axis2.Z, axis3.Z }
};
float[] vec = new float[] { pointRelPos.X, pointRelPos.Y, pointRelPos.Z };
SimultaneousEquations solve = SimultaneousEquations.Solve(mat, vec);
if (solve.Answers.Length == 3)
{
relValues = new decimal?[3];
try {
relValues[0] = (decimal)solve.Answers[0];
relValues[1] = (decimal)solve.Answers[1];
relValues[2] = (decimal)solve.Answers[2];
} catch (OverflowException) { relValues = null; }
}
}
values.SetValue(frame.Time, relValues);
}
SequenceData data = new SequenceData(values, null, PathEx.GiveName("RelPos", pointInfo.Name));
ret.Add(data);
}
return(ret);
}
/// <summary>
/// 線分と三角形が交点を持つかを求めます
/// </summary>
/// <param name="lineSource"></param>
/// <param name="lineDestination"></param>
/// <param name="vertex1"></param>
/// <param name="vertex2"></param>
/// <param name="vertex3"></param>
/// <param name="distance"></param>
/// <returns></returns>
public static bool GetCollisionTriangle(Vector3 lineSource, Vector3 lineDestination, Vector3 vertex1, Vector3 vertex2, Vector3 vertex3, out float distance)
{
// E + aD = (V2-V1)b + (V3-V1)c + V1
// aD + b(V1-V2) + c(V1-V3) = V1 - E
distance = 0;
Vector3 diff2 = vertex2 - vertex1;
Vector3 diff3 = vertex3 - vertex1;
Vector3 lineEdge = lineDestination - lineSource;
Vector3 rightConst = vertex1 - lineSource;
float[,] mat = new float[3, 3];
float[] vec = new float[3];
mat[0, 0] = lineEdge.X;
mat[1, 0] = lineEdge.Y;
mat[2, 0] = lineEdge.Z;
mat[0, 1] = -diff2.X;
mat[1, 1] = -diff2.Y;
mat[2, 1] = -diff2.Z;
mat[0, 2] = -diff3.X;
mat[1, 2] = -diff3.Y;
mat[2, 2] = -diff3.Z;
vec[0] = rightConst.X;
vec[1] = rightConst.Y;
vec[2] = rightConst.Z;
SimultaneousEquations se = SimultaneousEquations.Solve(mat, vec);
if (se.Answers.Length == 0)
{
distance = 0;
return(false);
}
for (int i = 0; i < 3; i++)
{
if (se.Answers[i] < 0 || se.Answers[i] >= 1)
{
distance = 0;
return(false);
}
}
// 三角形なので和が1以下
if (se.Answers[1] + se.Answers[2] >= 1)
{
distance = 0;
return(false);
}
distance = lineEdge.Length() * se.Answers[0];
return(true);
}
static bool getCrossPoint(Vector3 end1, Vector3 dir1, Vector3 end2, Vector3 dir2, out Vector3 point)
{
point = Vector3.Empty;
// 二線が交差していないと変な結果を出す
// むしろ二線の距離が最小となる位置を出すべきなのだろうか
float[,] mat = new float[2, 2];
float[] vec = new float[2];
float useX = Math.Abs(dir1.X) + Math.Abs(dir2.X);
float useY = Math.Abs(dir1.Y) + Math.Abs(dir2.Y);
float useZ = Math.Abs(dir1.Z) + Math.Abs(dir2.Z);
// C1+aV1=C2+bV2
// aV1-bV2=C2-C1
Vector3 endDif = end2 - end1;
if (useX < useY && useX < useZ)
{
mat[0, 0] = dir1.Z;
mat[0, 1] = -dir2.Z;
mat[1, 0] = dir1.Y;
mat[1, 1] = -dir2.Y;
vec[0] = endDif.Z;
vec[1] = endDif.Y;
}
else if (useY < useX && useY < useZ)
{
mat[0, 0] = dir1.X;
mat[0, 1] = -dir2.X;
mat[1, 0] = dir1.Z;
mat[1, 1] = -dir2.Z;
vec[0] = endDif.X;
vec[1] = endDif.Z;
}
else
{
mat[0, 0] = dir1.X;
mat[0, 1] = -dir2.X;
mat[1, 0] = dir1.Y;
mat[1, 1] = -dir2.Y;
vec[0] = endDif.X;
vec[1] = endDif.Y;
}
SimultaneousEquations se = SimultaneousEquations.Solve(mat, vec);
if (se.Answers.Length == 0)
{
return(false);
}
point = end1 + dir1 * se.Answers[0];
return(true);
}
/// <summary>
/// 二つの線分を結ぶ垂直な線分が引ける場合にその線分の端点を返します。
/// </summary>
/// <param title="end1">一方の線分の端点</param>
/// <param title="dir1">一方の線分の他の端点の相対座標</param>
/// <param title="end2">他方の線分の端点</param>
/// <param title="dir2">他方の線分の他の端点の相対座標</param>
/// <param title="pointOnLine1">二線分にとって垂直な線分の一方の端点</param>
/// <param title="pointOnLine2">二線分にとって垂直な線分の他方の端点</param>
/// <returns>引けた場合はtrue</returns>
public static bool GetNearestPointsInLines(Vector3 end1, Vector3 dir1, Vector3 end2, Vector3 dir2, out Vector3 pointOnLine1, out Vector3 pointOnLine2)
{
// min: |(e1+Ad1)-(e2+Bd2)|
// 両直線にとって垂直な線分で端点が各直線に含まれる
// (d1, (e1+Ad1)-(e2+Bd2)) = (d2, (e1+Ad1)-(e2+Bd2)) = 0
// A|d1|^2 - B(d1,d2) = (d1, e2-e1)
// A(d1, d2) - B|d2|^2 = (d2, e2-e1)
float[,] mat = new float[2, 2];
float[] vec = new float[2];
Vector3 endDiff = end2 - end1;
float dot = Vector3.Dot(dir1, dir2);
mat[0, 0] = dir1.LengthSq();
mat[0, 1] = -dot;
mat[1, 0] = dot;
mat[1, 1] = -dir2.LengthSq();
vec[0] = Vector3.Dot(dir1, endDiff);
vec[1] = Vector3.Dot(dir2, endDiff);
SimultaneousEquations se = SimultaneousEquations.Solve(mat, vec);
if (se.Answers.Length != 2)
{
pointOnLine1 = pointOnLine2 = Vector3.Empty;
return(false);
}
float dirRatio1 = se.Answers[0];
float dirRatio2 = se.Answers[1];
if (dirRatio1 < 0 || dirRatio2 < 0 || dirRatio1 >= 1 || dirRatio2 >= 1)
{
pointOnLine1 = pointOnLine2 = Vector3.Empty;
return(false);
}
pointOnLine1 = end1 + dir1 * dirRatio1;
pointOnLine2 = end2 + dir2 * dirRatio2;
return(true);
}
