Path *
pathNew( Vertex* origin, int init_size, int expand_delta ) {
Path *this = (Path*)malloc(sizeof(Path));
this->vertices = vecNew( init_size, expand_delta );
this->edges = vecNew( init_size, expand_delta );
/*
* A path is an alternating series of (vertex, edge, vertex, edge, vertex)
* elements. As such a complete path always has one more vertices than
* edges. One way to deal with this inconveniently dangling Vertex is to
* specify it at path initialization.
*/
vecAdd( this->vertices, origin );
return this;
}
static void
get_target_position(stateVector *st, double look, double yaw,
double sr, vector *targPos)
{
vector relPos = vecNew(sin(yaw), -sin(look)*cos(yaw), -cos(look)*cos(yaw));
// relPos unit vector points from s/c to targPos.
// Rotate into earth axes
vector look_matrix[3];
look_matrix[2] = st->pos;
vecNormalize(&look_matrix[2]);
vector v = st->vel;
vecNormalize(&v);
vecCross(look_matrix[2],v,&look_matrix[1]);
vecCross(look_matrix[1],look_matrix[2],&look_matrix[0]);
vecMul(look_matrix,relPos,&relPos);
// scale relPos so it reaches from s/c to targPos
vecScale(&relPos,sr);
vecAdd(relPos,st->pos,targPos);
}