//point on plane
public RelativePosition( Point2D p, Segment seg )
{
m_Segment = seg;
Point2D BtoPos = p - m_Segment.B;
Point2D AtoPos = p - m_Segment.A;
if( Point2D.Dot( BtoPos, m_Segment.AtoBDir ) > 0.0 )
{
m_fTheta = DMS.FixAngle(BtoPos.Theta - m_Segment.AtoBDir.Theta);
m_fDistance = BtoPos.R;
m_fClosestPt = m_Segment.Length;
} /* if */
else if( Point2D.Dot( AtoPos, m_Segment.AtoBDir ) < 0.0 )
{
m_fTheta = DMS.FixAngle(AtoPos.Theta - m_Segment.AtoBDir.Theta);
m_fDistance = AtoPos.R;
m_fClosestPt = 0.0;
} /* else if */
else
{
double
fAnglePosAB = DMS.FixAngle(AtoPos.Theta - m_Segment.AtoBDir.Theta);
m_fDistance = Math.Sin( fAnglePosAB ) * AtoPos.R;
m_fClosestPt = Math.Cos( fAnglePosAB ) * AtoPos.R;
m_fTheta = m_fDistance > 0.0 ? DMS.QUARTERTAU : -DMS.QUARTERTAU;
m_fDistance = Math.Abs( m_fDistance );
} /* else */
}
private Segment m_Segment; //which segment we're talking about
#endregion Fields
#region Constructors
//point on sphere
public RelativePosition( Point3D p, Segment seg )
{
m_Segment = seg;
Point3D PosOnSphere = m_Segment.Arot.Inverse.Rotate( p ); //we think of our segment starting at z and rotating towards x;
double fAngle = DMS.FixAnglePositive(Math.Atan2(PosOnSphere.X, PosOnSphere.Z));
//test if closest orthoganally
if( fAngle < m_Segment.Length )
{
m_fTheta = PosOnSphere.Y > 0 ? DMS.QUARTERTAU : -DMS.QUARTERTAU;
m_fClosestPt = fAngle;
m_fDistance = Math.Abs(Math.Asin( PosOnSphere.Y ));
} /* if */
//angle to A (z-axis) is TWO_PI-fAngle, angle to B (end of rot.) is fAngle-length
// test if point on sphere is closer to z axis then to end of rot.
else if( (DMS.TAU - fAngle) < (fAngle - m_Segment.Length) )
{
m_fTheta = PosOnSphere.Theta;
m_fClosestPt = 0.0;
m_fDistance = PosOnSphere.Phi;
} /* else if */
// otherwise point on sphere is closer to end of rotation then to z axis
else
{
//rotate posonsphere about y (from x to z) by grSegments[dtCurrent.nIdx].length
PosOnSphere = new Point3D(
Math.Sin(Math.Atan2(PosOnSphere.X, PosOnSphere.Z)-m_Segment.Length) * Math.Sqrt(1.0-PosOnSphere.Y*PosOnSphere.Y),
PosOnSphere.Y,
Math.Cos(Math.Atan2(PosOnSphere.X, PosOnSphere.Z)-m_Segment.Length) * Math.Sqrt(1.0-PosOnSphere.Y*PosOnSphere.Y) );
//now it's same as before except for fClosestPt
m_fTheta = PosOnSphere.Theta;
m_fClosestPt = m_Segment.Length;
m_fDistance = PosOnSphere.Phi;
} /* else if */
}
public int IndexOf(Segment s)
{
return m_Segments.IndexOf(s);
}
public bool bNearerSegmentOnSphereExists( Point3D pos, double fCriteria )
{
if( m_LastCloserSegment != null &&
RelativePosition.IsNearerOnSphere( pos, m_LastCloserSegment, fCriteria ) )
{
return true;
}
foreach( Segment seg in m_Segments )
{
if (RelativePosition.IsNearerOnSphere(pos, seg, fCriteria))
{
m_LastCloserSegment = seg;
return true;
}
}
return false;
}
public static bool IsNearerOnSphere( Point3D p, Segment seg, double criteria )
{
Point3D PosOnSphere = seg.Arot.Inverse.Rotate( p ); //we think of our segment starting at z and rotating towards x;
double fAngle = DMS.FixAnglePositive(Math.Atan2(PosOnSphere.X, PosOnSphere.Z));
//test if closest orthoganally
if( fAngle < seg.Length )
{
return (Math.Abs(Math.Asin(PosOnSphere.Y)) * seg.Strength < criteria - DMS.EPSILON);
} /* if */
//angle to A (z-axis) is TWO_PI-fAngle, angle to B (end of rot.) is fAngle-length
// test if point on sphere is closer to z axis then to end of rot.
else if( (DMS.TAU - fAngle) < (fAngle - seg.Length) )
{
return (PosOnSphere.Phi * seg.Strength < criteria - DMS.EPSILON);
} /* else if */
// otherwise point on sphere is closer to end of rotation then to z axis
else
{
//rotate posonsphere about y (from x to z) by grSegments[dtCurrent.nIdx].length
PosOnSphere = new Point3D(
Math.Sin(Math.Atan2(PosOnSphere.X, PosOnSphere.Z)-seg.Length) * Math.Sqrt(1.0-PosOnSphere.Y*PosOnSphere.Y),
PosOnSphere.Y,
Math.Cos(Math.Atan2(PosOnSphere.X, PosOnSphere.Z)-seg.Length) * Math.Sqrt(1.0-PosOnSphere.Y*PosOnSphere.Y) );
//now it's same as before except for fClosestPt
return (PosOnSphere.Phi * seg.Strength < criteria - DMS.EPSILON);
} /* else if */
}
