void DoSegment(int i,double *V0, double A)
{
double d,t,angle;
d = dist[i]; // get distance for this segment
// determine how much time will be required to go this distance
// at a starting Velocity of V0 while accelerating at A
if (A!=0)
{
// must solve quadratic to determine time at end
t = (-(*V0) + sqrt((*V0)*(*V0) + 2.0f*A*d))/A;
}
else
{
// constant velocity case is simple
t = d/(*V0);
}
// determine the direction of the segment so we can control the tagential knife
angle = FindAngle(pointx[i]-pointx[i-1],pointy[i]-pointy[i-1]);
x0=x1; // this segment starts at the beg of previous
y0=y1;
a0=a1;
x1=pointx[i]*RESX;
y1=pointy[i]*RESY;
a1=angle*RESA;
InterpolateLinear(0.0, 0.5*A/d, *V0/d, 0.0, t); // add to coordinated motion buffer
*V0 += t * A; // return new final velocity
}
int SpecialSyreen(pPlayer pl)
{
s16 b;
s16 angle;
pPlayer opp=(pPlayer)pl->opp;
int ret=0;
s32 dist=distanceBetweenPoints(pl->object.xpos,pl->object.ypos,opp->object.xpos,
opp->object.ypos);
if (dist<208&&(!(opp->ship_flags & CREW_IMMUNE)))
{
s8 crew_loss;
crew_loss = ((8 * (208 - dist)/ 209));
if (crew_loss >= opp->crew)
crew_loss = opp->crew - 1;
for (int i=0;i<crew_loss&&opp->crew-ret>1;i++)
{
b=nextWeapon(pl,4,12);
if (b>0)
{
pl->weapon[b].type=CREW;
pl->weapon[b].object.life=1;
pl->weapon[b].status=200;//GUESS
pl->weapon[b].damage=0;
pl->weapon[b].target=pl->opp;
pl->weapon[b].parent=pl;
pl->weapon[b].damageparent=1;
pl->weapon[b].movefunc=&MoveCrew;
pl->weapon[b].hitfunc=&HitCrew;
pl->weapon[b].object.ignorecollision=0;
pl->weapon[b].object.size=8;
pl->weapon[b].object.angle = 0;
pl->weapon[b].object.xspeed=0;
pl->weapon[b].object.yspeed=0;
pl->weapon[b].turn_wait=CREW_WAIT;
angle=FindAngle(opp->object.xpos,opp->object.ypos,pl->object.xpos,pl->object.ypos);
angle=ModifyAngle(angle,((i*2)-crew_loss)*20);
pl->weapon[b].object.xpos = opp->object.xpos+((s32)(opp->offset*3 * SIN[angle])>>8);
pl->weapon[b].object.ypos = opp->object.ypos-((s32)(opp->offset*3 * COS[angle])>>8);
drawOnScreen(&pl->weapon[b].object.xscreen,&pl->weapon[b].object.yscreen,
pl->weapon[b].object.xpos,pl->weapon[b].object.ypos,screenx,screeny,pl->weapon[b].object.size);
sprites[pl->weapon[b].sprite].attribute0 = COLOR_256 | SQUARE | ROTATION_FLAG |SIZE_DOUBLE | MODE_TRANSPARENT | pl->weapon[b].object.yscreen; //setup sprite info, 256 colour, shape and y-coord
sprites[pl->weapon[b].sprite].attribute1 =SIZE_8 | ROTDATA(pl->weapon[b].sprite) | pl->weapon[b].object.xscreen;
sprites[pl->weapon[b].sprite].attribute2 = pl->SpriteStart+66 | PRIORITY(1);
ret++;
}
} //loop
EXPORT BOOL_T UpdateDescStraight(
int inx,
int e0,
int e1,
int ln,
int an,
descData_p desc,
long pivot )
{
coOrd mid;
if ( inx == e0 || inx == e1 ) {
*(DIST_T*)desc[ln].valueP = FindDistance( *(coOrd*)desc[e0].valueP, *(coOrd*)desc[e1].valueP );
*(ANGLE_T*)desc[an].valueP = FindAngle( *(coOrd*)desc[e0].valueP, *(coOrd*)desc[e1].valueP );
if ( inx == e0 )
desc[e1].mode |= DESC_CHANGE;
else
desc[e0].mode |= DESC_CHANGE;
desc[ln].mode |= DESC_CHANGE;
desc[an].mode |= DESC_CHANGE;
} else if ( inx == ln || inx == an ) {
if ( inx == ln && *(DIST_T*)desc[ln].valueP <= minLength ) {
ErrorMessage( MSG_OBJECT_TOO_SHORT );
*(DIST_T*)desc[ln].valueP = FindDistance( *(coOrd*)desc[e0].valueP, *(coOrd*)desc[e1].valueP );
desc[ln].mode |= DESC_CHANGE;
return FALSE;
}
switch (pivot) {
case DESC_PIVOT_FIRST:
Translate( (coOrd*)desc[e1].valueP, *(coOrd*)desc[e0].valueP, *(ANGLE_T*)desc[an].valueP, *(DIST_T*)desc[ln].valueP );
desc[e1].mode |= DESC_CHANGE;
break;
case DESC_PIVOT_SECOND:
Translate( (coOrd*)desc[e0].valueP, *(coOrd*)desc[e1].valueP, *(ANGLE_T*)desc[an].valueP+180.0, *(DIST_T*)desc[ln].valueP );
desc[e0].mode |= DESC_CHANGE;
break;
case DESC_PIVOT_MID:
mid.x = (((coOrd*)desc[e0].valueP)->x+((coOrd*)desc[e1].valueP)->x)/2.0;
mid.y = (((coOrd*)desc[e0].valueP)->y+((coOrd*)desc[e1].valueP)->y)/2.0;
Translate( (coOrd*)desc[e0].valueP, mid, *(ANGLE_T*)desc[an].valueP+180.0, *(DIST_T*)desc[ln].valueP/2.0 );
Translate( (coOrd*)desc[e1].valueP, mid, *(ANGLE_T*)desc[an].valueP, *(DIST_T*)desc[ln].valueP/2.0 );
desc[e0].mode |= DESC_CHANGE;
desc[e1].mode |= DESC_CHANGE;
break;
default:
break;
}
} else {
return FALSE;
}
return TRUE;
}
bool IsOrthonormal(const Matrix4 &m)
{
// Axis components
Vector3 xAxis(m.xX, m.xY, m.xZ);
Vector3 yAxis(m.yX, m.yY, m.yZ);
Vector3 zAxis(m.zX, m.zY, m.zZ);
// Angle relation ships between the axis
float xyTheta = FindAngle(xAxis, yAxis);
float yzTheta = FindAngle(yAxis, zAxis);
float xzTheta = FindAngle(xAxis, zAxis);
// Determine if all axis are perpendicular to each other
if (Equal(xyTheta, 90.0f) == true
&& Equal(yzTheta, 90.0f) == true
&& Equal(xzTheta, 90.0f) == true)
{
// Orthonormal Matrix4 as all axis are perpidicular.
return true;
}
// Fall through if axis are not perpindicular
return false;
}
int FireArilou(pPlayer pl)
{
pPlayer opp=(pPlayer)pl->opp;
play_sfx(&arilou_fire,pl->plr-1);
for (int b=0;b<4;b++)
{
pl->weapon[b].type=LASER;
pl->weapon[b].object.life=2;
pl->weapon[b].damage=-1;
pl->weapon[b].target=pl->opp;
pl->weapon[b].parent=pl;
pl->weapon[b].damageparent=0;
pl->weapon[b].movefunc=0;
pl->weapon[b].hitfunc=0;
pl->weapon[b].object.ignorecollision=0;
pl->weapon[b].object.size=32;//(b==3?8:32);
pl->weapon[b].object.angle = FindAngle(pl->object.xpos,pl->object.ypos,opp->object.xpos,opp->object.ypos);
s32 off=(b==0?13:13+(b*32));
pl->weapon[b].object.xspeed=0;
pl->weapon[b].object.yspeed=0;
pl->weapon[b].object.xpos = pl->object.xpos+((off * (s32)SIN[pl->weapon[b].object.angle])>>8);
pl->weapon[b].object.ypos = pl->object.ypos-((off * (s32)COS[pl->weapon[b].object.angle])>>8);
drawOnScreen(&pl->weapon[b].object.xscreen,&pl->weapon[b].object.yscreen,
pl->weapon[b].object.xpos,pl->weapon[b].object.ypos,screenx,screeny,pl->weapon[b].object.size);
sprites[pl->weapon[b].sprite].attribute0 = COLOR_256 | SQUARE | ROTATION_FLAG |SIZE_DOUBLE | MODE_TRANSPARENT | pl->weapon[b].object.yscreen; //setup sprite info, 256 colour, shape and y-coord
sprites[pl->weapon[b].sprite].attribute1 =SIZE_32 | ROTDATA(pl->weapon[b].sprite) | pl->weapon[b].object.xscreen;
sprites[pl->weapon[b].sprite].attribute2 = pl->SpriteStart+64 | PRIORITY(1);
}
return 1;
}
EXPORT STATUS_T CreateCurve(
wAction_t action,
coOrd pos,
BOOL_T track,
wDrawColor color,
DIST_T width,
long mode,
curveMessageProc message )
{
DIST_T d;
ANGLE_T a;
static coOrd pos0;
int inx;
switch ( action ) {
case C_START:
DYNARR_SET( trkSeg_t, tempSegs_da, 8 );
switch ( curveMode ) {
case crvCmdFromEP1:
InfoMessage( _("Drag from End-Point in direction of curve") );
break;
case crvCmdFromTangent:
InfoMessage( _("Drag from End-Point to Center") );
break;
case crvCmdFromCenter:
InfoMessage( _("Drag from Center to End-Point") );
break;
case crvCmdFromChord:
InfoMessage( _("Drag to other end of chord") );
break;
}
return C_CONTINUE;
case C_DOWN:
for ( inx=0; inx<8; inx++ ) {
tempSegs(inx).color = wDrawColorBlack;
tempSegs(inx).width = 0;
}
tempSegs_da.cnt = 0;
SnapPos( &pos );
pos0 = pos;
switch (mode) {
case crvCmdFromEP1:
tempSegs(0).type = (track?SEG_STRTRK:SEG_STRLIN);
tempSegs(0).color = color;
tempSegs(0).width = width;
message( _("Drag to set angle") );
break;
case crvCmdFromTangent:
case crvCmdFromCenter:
tempSegs(0).type = SEG_STRLIN;
tempSegs(1).type = SEG_CRVLIN;
tempSegs(1).u.c.radius = mainD.scale*0.05;
tempSegs(1).u.c.a0 = 0;
tempSegs(1).u.c.a1 = 360;
tempSegs(2).type = SEG_STRLIN;
message( mode==crvCmdFromTangent?_("Drag from End-Point to Center"):_("Drag from Center to End-Point") );
break;
case crvCmdFromChord:
tempSegs(0).type = (track?SEG_STRTRK:SEG_STRLIN);
tempSegs(0).color = color;
tempSegs(0).width = width;
message( _("Drag to other end of chord") );
break;
}
tempSegs(0).u.l.pos[0] = pos;
return C_CONTINUE;
case C_MOVE:
tempSegs(0).u.l.pos[1] = pos;
d = FindDistance( pos0, pos );
a = FindAngle( pos0, pos );
switch ( mode ) {
case crvCmdFromEP1:
message( _("Angle=%0.3f"), PutAngle(a) );
tempSegs_da.cnt = 1;
break;
case crvCmdFromTangent:
message( _("Radius=%s Angle=%0.3f"), FormatDistance(d), PutAngle(a) );
tempSegs(1).u.c.center = pos;
DrawArrowHeads( &tempSegs(2), pos0, FindAngle(pos0,pos)+90, TRUE, wDrawColorBlack );
tempSegs_da.cnt = 7;
break;
case crvCmdFromCenter:
message( _("Radius=%s Angle=%0.3f"), FormatDistance(d), PutAngle(a) );
tempSegs(1).u.c.center = pos0;
DrawArrowHeads( &tempSegs(2), pos, FindAngle(pos,pos0)+90, TRUE, wDrawColorBlack );
tempSegs_da.cnt = 7;
break;
case crvCmdFromChord:
message( _("Length=%s Angle=%0.3f"), FormatDistance(d), PutAngle(a) );
if ( d > mainD.scale*0.25 ) {
pos.x = (pos.x+pos0.x)/2.0;
pos.y = (pos.y+pos0.y)/2.0;
DrawArrowHeads( &tempSegs(1), pos, FindAngle(pos,pos0)+90, TRUE, wDrawColorBlack );
tempSegs_da.cnt = 6;
} else {
tempSegs_da.cnt = 1;
}
break;
}
return C_CONTINUE;
case C_UP:
switch (mode) {
case crvCmdFromEP1:
DrawArrowHeads( &tempSegs(1), pos, FindAngle(pos,pos0)+90, TRUE, drawColorRed );
tempSegs_da.cnt = 6;
break;
case crvCmdFromChord:
tempSegs(1).color = drawColorRed;
case crvCmdFromTangent:
case crvCmdFromCenter:
tempSegs(2).color = drawColorRed;
tempSegs(3).color = drawColorRed;
tempSegs(4).color = drawColorRed;
tempSegs(5).color = drawColorRed;
tempSegs(6).color = drawColorRed;
break;
}
message( _("Drag on Red arrows to adjust curve") );
return C_CONTINUE;
default:
return C_CONTINUE;
}
}
static STATUS_T CmdCurve( wAction_t action, coOrd pos )
{
track_p t;
DIST_T d;
static int segCnt;
STATUS_T rc = C_CONTINUE;
switch (action) {
case C_START:
curveMode = (long)commandContext;
Da.state = -1;
tempSegs_da.cnt = 0;
return CreateCurve( action, pos, TRUE, wDrawColorBlack, 0, curveMode, InfoMessage );
case C_TEXT:
if ( Da.state == 0 )
return CreateCurve( action, pos, TRUE, wDrawColorBlack, 0, curveMode, InfoMessage );
else
return C_CONTINUE;
case C_DOWN:
if ( Da.state == -1 ) {
SnapPos( &pos );
Da.pos0 = pos;
Da.state = 0;
return CreateCurve( action, pos, TRUE, wDrawColorBlack, 0, curveMode, InfoMessage );
} else {
tempSegs_da.cnt = segCnt;
return C_CONTINUE;
}
case C_MOVE:
mainD.funcs->options = wDrawOptTemp;
DrawSegs( &mainD, zero, 0.0, &tempSegs(0), tempSegs_da.cnt, trackGauge, wDrawColorBlack );
if ( Da.state == 0 ) {
SnapPos( &pos );
Da.pos1 = pos;
rc = CreateCurve( action, pos, TRUE, wDrawColorBlack, 0, curveMode, InfoMessage );
} else {
SnapPos( &pos );
PlotCurve( curveMode, Da.pos0, Da.pos1, pos, &Da.curveData, TRUE );
if (Da.curveData.type == curveTypeStraight) {
tempSegs(0).type = SEG_STRTRK;
tempSegs(0).u.l.pos[0] = Da.pos0;
tempSegs(0).u.l.pos[1] = Da.curveData.pos1;
tempSegs_da.cnt = 1;
InfoMessage( _("Straight Track: Length=%s Angle=%0.3f"),
FormatDistance(FindDistance( Da.pos0, Da.curveData.pos1 )),
PutAngle(FindAngle( Da.pos0, Da.curveData.pos1 )) );
} else if (Da.curveData.type == curveTypeNone) {
tempSegs_da.cnt = 0;
InfoMessage( _("Back") );
} else if (Da.curveData.type == curveTypeCurve) {
tempSegs(0).type = SEG_CRVTRK;
tempSegs(0).u.c.center = Da.curveData.curvePos;
tempSegs(0).u.c.radius = Da.curveData.curveRadius;
tempSegs(0).u.c.a0 = Da.curveData.a0;
tempSegs(0).u.c.a1 = Da.curveData.a1;
tempSegs_da.cnt = 1;
d = D2R(Da.curveData.a1);
if (d < 0.0)
d = 2*M_PI+d;
if ( d*Da.curveData.curveRadius > mapD.size.x+mapD.size.y ) {
ErrorMessage( MSG_CURVE_TOO_LARGE );
tempSegs_da.cnt = 0;
Da.curveData.type = curveTypeNone;
mainD.funcs->options = 0;
return C_CONTINUE;
}
InfoMessage( _("Curved Track: Radius=%s Angle=%0.3f Length=%s"),
FormatDistance(Da.curveData.curveRadius), Da.curveData.a1,
FormatDistance(Da.curveData.curveRadius*d) );
}
}
DrawSegs( &mainD, zero, 0.0, &tempSegs(0), tempSegs_da.cnt, trackGauge, wDrawColorBlack );
mainD.funcs->options = 0;
return rc;
case C_UP:
mainD.funcs->options = wDrawOptTemp;
DrawSegs( &mainD, zero, 0.0, &tempSegs(0), tempSegs_da.cnt, trackGauge, wDrawColorBlack );
if (Da.state == 0) {
SnapPos( &pos );
Da.pos1 = pos;
Da.state = 1;
CreateCurve( action, pos, TRUE, wDrawColorBlack, 0, curveMode, InfoMessage );
DrawSegs( &mainD, zero, 0.0, &tempSegs(0), tempSegs_da.cnt, trackGauge, wDrawColorBlack );
mainD.funcs->options = 0;
segCnt = tempSegs_da.cnt;
InfoMessage( _("Drag on Red arrows to adjust curve") );
return C_CONTINUE;
} else {
mainD.funcs->options = 0;
tempSegs_da.cnt = 0;
Da.state = -1;
if (Da.curveData.type == curveTypeStraight) {
if ((d=FindDistance( Da.pos0, Da.curveData.pos1 )) <= minLength) {
ErrorMessage( MSG_TRK_TOO_SHORT, "Curved ", PutDim(fabs(minLength-d)) );
return C_TERMINATE;
}
UndoStart( _("Create Straight Track"), "newCurve - straight" );
t = NewStraightTrack( Da.pos0, Da.curveData.pos1 );
UndoEnd();
} else if (Da.curveData.type == curveTypeCurve) {
if ((d= Da.curveData.curveRadius * Da.curveData.a1 *2.0*M_PI/360.0) <= minLength) {
ErrorMessage( MSG_TRK_TOO_SHORT, "Curved ", PutDim(fabs(minLength-d)) );
return C_TERMINATE;
}
UndoStart( _("Create Curved Track"), "newCurve - curve" );
t = NewCurvedTrack( Da.curveData.curvePos, Da.curveData.curveRadius,
Da.curveData.a0, Da.curveData.a1, 0 );
UndoEnd();
} else {
return C_ERROR;
}
DrawNewTrack( t );
return C_TERMINATE;
}
case C_REDRAW:
if ( Da.state >= 0 ) {
mainD.funcs->options = wDrawOptTemp;
DrawSegs( &mainD, zero, 0.0, &tempSegs(0), tempSegs_da.cnt, trackGauge, wDrawColorBlack );
mainD.funcs->options = 0;
}
return C_CONTINUE;
case C_CANCEL:
if (Da.state == 1) {
mainD.funcs->options = wDrawOptTemp;
DrawSegs( &mainD, zero, 0.0, &tempSegs(0), tempSegs_da.cnt, trackGauge, wDrawColorBlack );
mainD.funcs->options = 0;
tempSegs_da.cnt = 0;
}
Da.state = -1;
return C_CONTINUE;
}
return C_CONTINUE;
}
static STATUS_T CmdHandLaidTurnout( wAction_t action, coOrd pos )
{
ANGLE_T angle, angle2, angle3, reverseR, pointA, reverseA1, angle0;
EPINX_T ep, ep1, ep2, ep2a=-1, ep2b=-1, pointEp0, pointEp1;
DIST_T dist, reverseD, pointD;
coOrd off, intersectP;
coOrd pointP, pointC, pointP1, reverseC, point0;
track_p trk, trk1, trk2, trk2a=NULL, trk2b=NULL, pointT;
trkSeg_p segP;
BOOL_T right;
track_p trks[4], *trkpp;
switch (action) {
case C_START:
InfoMessage( _("Place frog and drag angle") );
DYNARR_SET( trkSeg_t, tempSegs_da, 1 );
Dhlt.state = 0;
Dhlt.normalT = NULL;
tempSegs_da.cnt = 0;
DYNARR_SET( trkSeg_t, tempSegs_da, 2 );
tempSegs(0).color = drawColorBlack;
tempSegs(0).width = 0;
tempSegs(1).color = drawColorBlack;
tempSegs(1).width = 0;
return C_CONTINUE;
case C_DOWN:
if (Dhlt.state == 0) {
if ((Dhlt.normalT = OnTrack( &pos, TRUE, TRUE )) == NULL)
break;
if ( QueryTrack( Dhlt.normalT, Q_NOT_PLACE_FROGPOINTS ) ) {
ErrorMessage( MSG_CANT_PLACE_FROGPOINTS, _("frog") );
Dhlt.normalT = NULL;
break;
}
Dhlt.normalP = Dhlt.reverseP = Dhlt.reverseP1 = pos;
Dhlt.normalA = GetAngleAtPoint( Dhlt.normalT, Dhlt.normalP, NULL, NULL );
InfoMessage( _("Drag to set angle") );
DrawLine( &tempD, Dhlt.reverseP, Dhlt.reverseP1, 0, wDrawColorBlack );
Dhlt.state = 1;
pointC = pointP = pointP1 = reverseC = zero;
return C_CONTINUE;
}
case C_MOVE:
case C_UP:
if (Dhlt.normalT == NULL)
break;
if (Dhlt.state == 1) {
DrawLine( &tempD, Dhlt.reverseP, Dhlt.reverseP1, 0, wDrawColorBlack );
Dhlt.reverseP1 = pos;
Dhlt.reverseA = FindAngle( Dhlt.reverseP, Dhlt.reverseP1 );
Dhlt.frogA = NormalizeAngle( Dhlt.reverseA - Dhlt.normalA );
/*printf( "RA=%0.3f FA=%0.3f ", Dhlt.reverseA, Dhlt.frogA );*/
if (Dhlt.frogA > 270.0) {
Dhlt.frogA = 360.0-Dhlt.frogA;
right = FALSE;
} else if (Dhlt.frogA > 180) {
Dhlt.frogA = Dhlt.frogA - 180.0;
Dhlt.normalA = NormalizeAngle( Dhlt.normalA + 180.0 );
/*ep = Dhlt.normalEp0; Dhlt.normalEp0 = Dhlt.normalEp1; Dhlt.normalEp1 = ep;*/
right = TRUE;
} else if (Dhlt.frogA > 90.0) {
Dhlt.frogA = 180.0 - Dhlt.frogA;
Dhlt.normalA = NormalizeAngle( Dhlt.normalA + 180.0 );
/*ep = Dhlt.normalEp0; Dhlt.normalEp0 = Dhlt.normalEp1; Dhlt.normalEp1 = ep;*/
right = FALSE;
} else {
right = TRUE;
}
/*printf( "NA=%0.3f FA=%0.3f R=%d\n", Dhlt.normalA, Dhlt.frogA, right );*/
Dhlt.frogNo = tan(D2R(Dhlt.frogA));
if (Dhlt.frogNo > 0.01)
Dhlt.frogNo = 1.0/Dhlt.frogNo;
else
Dhlt.frogNo = 0.0;
if (action == C_MOVE) {
if (Dhlt.frogNo != 0) {
InfoMessage( _("Angle = %0.2f Frog# = %0.2f"), Dhlt.frogA, Dhlt.frogNo );
} else {
InfoMessage( _("Frog angle is too close to 0") );
}
} else {
InfoMessage( _("Select point position") );
Dhlt.state = 2;
Translate( &Dhlt.reverseP, Dhlt.reverseP, Dhlt.normalA+(right?+90:-90), trackGauge );
Translate( &Dhlt.reverseP1, Dhlt.reverseP1, Dhlt.normalA+(right?+90:-90), trackGauge );
}
DrawLine( &tempD, Dhlt.reverseP, Dhlt.reverseP1, 0, wDrawColorBlack );
return C_CONTINUE;
} else if ( Dhlt.state == 2 ) {
DrawSegs( &tempD, zero, 0.0, &tempSegs(0), tempSegs_da.cnt, trackGauge, wDrawColorBlack );
tempSegs_da.cnt = 0;
pointP = pos;
if ((pointT = OnTrack( &pointP, TRUE, TRUE )) == NULL)
break;
if ( QueryTrack( pointT, Q_NOT_PLACE_FROGPOINTS ) ) {
ErrorMessage( MSG_CANT_PLACE_FROGPOINTS, _("points") );
break;
}
dist = FindDistance( Dhlt.normalP, pointP );
pointA = GetAngleAtPoint( pointT, pointP, &pointEp0, &pointEp1 );
angle = NormalizeAngle( pointA + 180.0 - Dhlt.reverseA );
PTRACE(( "rA=%0.1f pA=%0.1f a=%0.1f ", Dhlt.reverseA, pointA, angle ))
if ( angle > 90.0 && angle < 270.0 ) {
pointA = NormalizeAngle( pointA + 180.0 );
angle = NormalizeAngle( angle + 180.0 );
PTRACE(( " {pA=%0.1f a=%0.1f} ", pointA, angle ))
} else {
void DoEllipse(void)
{
int i,ibeg,iend;
double dist_beg,dist_end,V0,ta,da,length,Time0,Time1,MaxAp,MaxVp,angle;
Time0=Time_sec();
// first compute total length of path and build tables of info
length = ArcLength(ThetaStart,ThetaEnd,MAXN);
Time1=Time_sec();
printf("Length = %.9f Compute Time %.0f us\n",length, (Time1-Time0)*1e6);
// time to achieve max vel
ta = MaxV/MaxA;
// dist to achieve max vel
da = (0.5 * MaxA * ta) * ta;
printf("Accel Time %f Accel dist %f\n",ta,da);
// first move from where we are to starting point on ellipse
x0=ch0->Dest; // starting point is where we currently are
y0=ch1->Dest;
z0=ch2->Dest;
a0=ch3->Dest;
x1 = pointx[0]*RESX;
y1 = pointy[0]*RESY;
z1=z0; // keep z and a the same
a1=a0;
DoLinear();
// now rotate knife to be at the right angle for the first segment
angle = FindAngle(pointx[1]-pointx[0],pointy[1]-pointy[0]);
printf("angle=%f %f %f\n",angle,pointx[1]-pointx[0],pointy[1]-pointy[0]);
x0=x1; // xyz doesn't move
y0=y1;
z0=z1;
a0=a1;
a1=angle*RESA; // rotate knife to proper angle
DoLinear();
// search to find segments needed to accellerate/decelerate
// searches forward and backward throug the segments simultaneously
// until either the begiining and ending are sufficient to accel/deccel
// or we meet in the middle and run out of distance
ibeg=0;
iend=MAXN;
dist_beg = dist_end = 0.0;
do
{
if (dist_beg < dist_end)
ibeg++;
else
iend--;
dist_beg = sum[ibeg];
dist_end = sum[MAXN]-sum[iend];
// printf("ibeg=%d iend=%d dist beg %.3f dist_end %.3f\n",ibeg,iend,dist_beg,dist_end);
}
while ((dist_beg<da || dist_end<da) && ibeg<iend);
// if distance available is less than required accel distance
// then reduce the max velocity to what is achievable
if (ibeg>=iend)
{
if (dist_beg<dist_end)
MaxVp=sqrt(2.0*MaxA*dist_beg);
else
MaxVp=sqrt(2.0*MaxA*dist_end);
}
else
{
MaxVp=MaxV;
}
// adjust the acceleration to accelerate in exactly the beginning distance
MaxAp = 0.5*MaxVp*MaxVp/dist_beg;
// create all the segments
f=fopen("C:\\temp\\kflopdata.txt","wt");
V0=0.0; // start from stop
for (i=1;i<=ibeg;i++)
DoSegment(i,&V0,MaxAp); // accel segments
for (;i<=iend;i++)
DoSegment(i,&V0,0.0); // constant velocity
// adjust the acceleration to accelerate in exactly the ending distance
MaxAp = 0.5*MaxVp*MaxVp/dist_end;
for (;i<=MAXN;i++)
DoSegment(i,&V0,-MaxAp); // decel segments
fclose(f);
}
