// gets the vector projection of pt on this segment, clamped
public stpoint getProjection(stpoint pt)
{
// center the problem
Vector2 ap = pt.xy - s1.xy;
Vector2 ab = s2.xy - s1.xy;
float ablen = ab.Length();
// if ablen is zero, quit
if (ablen == 0)
{
return(new stpoint(s1.xy, s1.t));
}
float plen = Vector2.Dot(ab, ap) / ablen;
if (plen < 0)
{
plen = 0;
}
else if (plen > ablen)
{
plen = ablen;
}
ap = ab * plen / ablen; // store the result in ap
if (speed == 0)
{
return(new stpoint(ap, pt.t));
}
double time = _s1.t + Vector2.Distance(ap, s1.xy) / speed;
return(new stpoint(ap, time));
}
public Trajectory insert(Trajectory other)
{
stsegment e1 = this[0];
stsegment e2 = other[0];
// if inserting onto a segment of len 0, return without calculating
if (e1.s1 == e1.s2)
{
return(insertAndClone(e1, e1));
}
stpoint pt = e1.getProjection(e2.s2);
stsegment newseg1 = new stsegment(e1.s1, pt);
stsegment newseg2 = new stsegment(pt, e1.s2);
return(insertAndClone(newseg1, newseg2));
}
public stsegment(float x1, float y1, double t1, float x2, float y2, double t2)
{
_s1 = new stpoint(x1, y1, t1);
_s2 = new stpoint(x2, y2, t2);
length = calclen();
}
public stsegment(stpoint s1, stpoint s2)
{
_s1 = s1;
_s2 = s2;
length = calclen();
}