public void LinkTo(SurfSegment seg)
{
if (EndPoint.X != seg.StartPoint.X ||
EndPoint.Y != seg.StartPoint.Y ||
EndPoint.Z != seg.StartPoint.Z ||
Height != seg.Height)
{
throw new ArgumentException("The ramp does not share endpoint/startpoint or height");
}
var cross = Vector3D.CrossProduct(GetDirection(), seg.GetDirection());
var dot = Vector3D.DotProduct(GetDirection(), seg.GetDirection());
var theta = dot / (GetDirection().Length *seg.GetDirection().Length);
if (theta.ApproximateEquals(1))
{
throw new ArgumentException("Ramp is Straight on");
}
else if (cross.Z.ApproximateEquals(0)) //vertical turn (up/down)
{
LinkVerticallyTo(seg);
}
else if (cross.X.ApproximateEquals(0) && cross.Y.ApproximateEquals(0)) //Horizontal flat turn?
{
LinkHorizontallyTo(seg);
}
}
private bool IsUpwardsTurn(SurfSegment seg)
{
var n = Vector3D.CrossProduct(this.GetDirection(), seg.GetDirection());
var k = Vector3D.CrossProduct(this.GetDirection(), n);
return(k.Z < 0);
}