296 lines
86 KiB
HolyC
296 lines
86 KiB
HolyC
|
#define THRUST 1000
|
|||
|
|
|||
|
Bool blast_off,plane_hit;
|
|||
|
|
|||
|
CMass m1, //Bottom of rocket
|
|||
|
m2, //Top of rocket
|
|||
|
m3; //Plane
|
|||
|
CSpring s;
|
|||
|
|
|||
|
#define ROCKET_HEIGHT 40
|
|||
|
#define GROUND_Y (GR_HEIGHT-3*FONT_HEIGHT)
|
|||
|
$BG,11$
|
|||
|
|
|||
|
$SP,"<1>",BI=1$
|
|||
|
|
|||
|
|
|||
|
|
|||
|
$SP,"<2>",BI=2$
|
|||
|
|
|||
|
|
|||
|
|
|||
|
$SP,"<3>",BI=3$
|
|||
|
|
|||
|
$BG$
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
|
|||
|
$SP,"",BI=4$
|
|||
|
|
|||
|
CDC *dc2;
|
|||
|
CMathODE *ode;
|
|||
|
|
|||
|
#define STATE_NOZZLE_ANGLE 0
|
|||
|
#define STATE_NOZZLE_ANGLE_VELOCITY 1
|
|||
|
#define STATE_NUM 2
|
|||
|
|
|||
|
CD3 target;
|
|||
|
F64 my_dbg,antispin_coefficient;
|
|||
|
|
|||
|
U0 DrawIt(CTask *task,CDC *dc)
|
|||
|
{
|
|||
|
I64 i,x,y,cx=GR_WIDTH/2,cy=GROUND_Y;
|
|||
|
F64 <EFBFBD>=Arg(m2.x-m1.x,m2.y-m1.y),
|
|||
|
nozzle_angle=ode->state[STATE_NOZZLE_ANGLE];
|
|||
|
|
|||
|
if (blast_off) {
|
|||
|
x=m1.x-10*Cos(<EFBFBD>+nozzle_angle);
|
|||
|
y=m1.y-10*Sin(<EFBFBD>+nozzle_angle);
|
|||
|
for (i=0;i<6;i++) {
|
|||
|
if ((i^winmgr.updates)&1)
|
|||
|
dc->color=YELLOW;
|
|||
|
else
|
|||
|
dc->color=RED;
|
|||
|
GrLine(dc,cx+(m1.x+i*Cos(<EFBFBD>-<EFBFBD>/2)),cy-(m1.y+i*Sin(<EFBFBD>-<EFBFBD>/2)),cx+x,cy-y);
|
|||
|
GrLine(dc,cx+(m1.x+i*Cos(<EFBFBD>+<EFBFBD>/2)),cy-(m1.y+i*Sin(<EFBFBD>+<EFBFBD>/2)),cx+x,cy-y);
|
|||
|
}
|
|||
|
|
|||
|
for (i=0;i<10;i++) {
|
|||
|
switch (RandU16&3) {
|
|||
|
case 0: dc2->color=WHITE; break;
|
|||
|
case 1: dc2->color=LTGRAY; break;
|
|||
|
case 2: dc2->color=DKGRAY; break;
|
|||
|
case 3: dc2->color=BLACK; break;
|
|||
|
}
|
|||
|
GrPlot(dc2,cx+(x+RandU16%12-6),cy-(y+RandU16%12-6));
|
|||
|
}
|
|||
|
}
|
|||
|
|
|||
|
if (plane_hit)
|
|||
|
Sprite3(dc,cx+m3.x,cy-m3.y,0,$IB,"<2>",BI=2$);
|
|||
|
else
|
|||
|
Sprite3(dc,cx+m3.x,cy-m3.y,0,$IB,"<1>",BI=1$);
|
|||
|
|
|||
|
if (blast_off && !plane_hit) {
|
|||
|
dc->color=ROP_COLLISION;
|
|||
|
dc->bkcolor=LTCYAN;
|
|||
|
dc->collision_cnt=0;
|
|||
|
Sprite3ZB(dc,cx+(m1.x+m2.x)/2,cy-(m1.y+m2.y)/2,0,$IB,"<3>",BI=3$,-<EFBFBD>);
|
|||
|
if (dc->collision_cnt>100) {
|
|||
|
Noise(1000,62,81);
|
|||
|
plane_hit=TRUE;
|
|||
|
} else
|
|||
|
Snd(22);
|
|||
|
} else if (!plane_hit)
|
|||
|
Snd;
|
|||
|
|
|||
|
dc->color=ROP_EQU;
|
|||
|
Sprite3(dc,0,GROUND_Y,0,$IB,"<4>",BI=4$);
|
|||
|
Sprite3ZB(dc,cx+(m1.x+m2.x)/2,cy-(m1.y+m2.y)/2,0,$IB,"<3>",BI=3$,-<EFBFBD>);
|
|||
|
|
|||
|
dc->color=RED;
|
|||
|
GrCircle(dc,cx+target.x,cy-target.y,5);
|
|||
|
|
|||
|
dc->color=BLUE;
|
|||
|
GrCircle(dc,cx+m3.x,cy-m3.y,5);
|
|||
|
|
|||
|
dc->color=BLACK;
|
|||
|
GrPrint(dc,0,FONT_HEIGHT,"%12.6f",my_dbg);
|
|||
|
}
|
|||
|
|
|||
|
U0 MyDerivative(CMathODE *,F64,F64 *state,F64 *DstateDt)
|
|||
|
{
|
|||
|
F64 d,discriminant,v,a,<EFBFBD>=Arg(m2.state->x-m1.state->x,m2.state->y-m1.state->y),
|
|||
|
D<EFBFBD>Dt,collision_estimate_t,target_heading,target_angle_error,
|
|||
|
desired_nozzle_angle;
|
|||
|
CD3 p,p_target,p_body;
|
|||
|
|
|||
|
//Unit vect pointing to top of rocket from bottom.
|
|||
|
D3Sub(&p_body,&m2.state->x,&m1.state->x);
|
|||
|
D3Unit(&p_body);
|
|||
|
|
|||
|
//D<>Dt lets us prevent too much spin.
|
|||
|
D<EFBFBD>Dt=antispin_coefficient*
|
|||
|
(m2.state->DyDt*p_body.x-m2.state->DxDt*p_body.y-
|
|||
|
m1.state->DyDt*p_body.x+m1.state->DxDt*p_body.y)/ROCKET_HEIGHT;
|
|||
|
|
|||
|
//p_target is vect from top of rocket to plane.
|
|||
|
D3Sub(&p_target,&m3.state->x,&m2.state->x);
|
|||
|
|
|||
|
//d=0.5at^2+vt
|
|||
|
d=D3Norm(&p_target);
|
|||
|
|
|||
|
D3Copy(&p,&p_target);
|
|||
|
D3Unit(&p);
|
|||
|
v=(m2.state->DxDt*p.x+m2.state->DyDt*p.y)-
|
|||
|
(m3.state->DxDt*p.x+m3.state->DyDt*p.y);
|
|||
|
|
|||
|
a=THRUST/(m1.mass+m2.mass);
|
|||
|
|
|||
|
discriminant=v*v+4*0.5*a*d;
|
|||
|
if (discriminant>0)
|
|||
|
collision_estimate_t=(-v+Sqrt(discriminant))/a;
|
|||
|
else
|
|||
|
collision_estimate_t=0;
|
|||
|
my_dbg=collision_estimate_t;
|
|||
|
|
|||
|
//Aim for projected pos of plane at time of impact.
|
|||
|
D3Copy(&p,&m3.state->DxDt);
|
|||
|
D3MulEqu(&p,collision_estimate_t);
|
|||
|
D3AddEqu(&p_target,&p);
|
|||
|
|
|||
|
D3Copy(&target,&p_target);
|
|||
|
D3AddEqu(&target,&m2.state->x);
|
|||
|
|
|||
|
target_heading=Arg(p_target.x,p_target.y);
|
|||
|
target_angle_error=Wrap(<EFBFBD>-target_heading); //Force to range [-<2D>,<2C>)
|
|||
|
desired_nozzle_angle=Clamp(50.0*D<EFBFBD>Dt+750*target_angle_error,-<EFBFBD>/8,<EFBFBD>/8);
|
|||
|
|
|||
|
//For realism we limit the speed the nozzle angle can change.
|
|||
|
DstateDt[STATE_NOZZLE_ANGLE]=state[STATE_NOZZLE_ANGLE_VELOCITY];
|
|||
|
DstateDt[STATE_NOZZLE_ANGLE_VELOCITY]=
|
|||
|
Clamp(10000*(desired_nozzle_angle-state[STATE_NOZZLE_ANGLE]),
|
|||
|
-1000,1000)-10.0*state[STATE_NOZZLE_ANGLE_VELOCITY]; //Damping
|
|||
|
|
|||
|
if (blast_off) {
|
|||
|
m1.DstateDt->DxDt+=THRUST*Cos(<EFBFBD>+state[STATE_NOZZLE_ANGLE]);
|
|||
|
m1.DstateDt->DyDt+=THRUST*Sin(<EFBFBD>+state[STATE_NOZZLE_ANGLE]);
|
|||
|
|
|||
|
m1.DstateDt->DyDt-=25; //Gravity
|
|||
|
m2.DstateDt->DyDt-=25;
|
|||
|
}
|
|||
|
|
|||
|
//For more realism reduce the mass of the rocket because of fuel.
|
|||
|
//You might also factor-in fuel slosh in the tank.
|
|||
|
|
|||
|
//To do this, you would have to set-up state vars for mass and
|
|||
|
//do A=F/m manually instead of relyin on $LK,"ODECallDerivative",A="MN:ODECallDerivative"$() to divide
|
|||
|
//by mass.
|
|||
|
}
|
|||
|
|
|||
|
U0 Init()
|
|||
|
{
|
|||
|
DocClear;
|
|||
|
"$$BG,LTCYAN$$%h*c",ToI64(GROUND_Y/FONT_HEIGHT),'\n';
|
|||
|
|
|||
|
blast_off=FALSE;
|
|||
|
plane_hit=FALSE;
|
|||
|
|
|||
|
do antispin_coefficient=PopUpRangeF64Exp(0.1,10.001,Sqrt(10),
|
|||
|
"%9.4f","Anti-spin Coefficient\n\n");
|
|||
|
while (!(0.1<=antispin_coefficient<10.001));
|
|||
|
|
|||
|
//We don't clear que links.
|
|||
|
MemSet(&m1.start,0,offset(CMass.end)-offset(CMass.start));
|
|||
|
m1.y=0;
|
|||
|
m1.mass=1.0;
|
|||
|
|
|||
|
MemSet(&m2.start,0,offset(CMass.end)-offset(CMass.start));
|
|||
|
m2.y=ROCKET_HEIGHT;
|
|||
|
m2.mass=1.0;
|
|||
|
|
|||
|
MemSet(&m3.start,0,offset(CMass.end)-offset(CMass.start));
|
|||
|
m3.y=400;
|
|||
|
m3.x=-300;
|
|||
|
m3.DxDt=50;
|
|||
|
m3.mass=1.0;
|
|||
|
|
|||
|
MemSet(&s.start,0,offset(CSpring.end)-offset(CSpring.start));
|
|||
|
s.end1=&m1;
|
|||
|
s.end2=&m2;
|
|||
|
s.rest_len=ROCKET_HEIGHT;
|
|||
|
s.const=10000;
|
|||
|
|
|||
|
ode->state[STATE_NOZZLE_ANGLE]=0;
|
|||
|
ode->state[STATE_NOZZLE_ANGLE_VELOCITY]=0;
|
|||
|
|
|||
|
DCFill;
|
|||
|
}
|
|||
|
|
|||
|
U0 TaskEndCB()
|
|||
|
{
|
|||
|
DCFill;
|
|||
|
SndTaskEndCB;
|
|||
|
}
|
|||
|
|
|||
|
U0 RocketScience()
|
|||
|
{
|
|||
|
SettingsPush; //See $LK,"SettingsPush",A="MN:SettingsPush"$
|
|||
|
Fs->text_attr=YELLOW<<4+BLUE;
|
|||
|
MenuPush(
|
|||
|
"File {"
|
|||
|
" Abort(,CH_SHIFT_ESC);"
|
|||
|
" Exit(,CH_ESC);"
|
|||
|
"}"
|
|||
|
"Play {"
|
|||
|
" Restart(,'\n');"
|
|||
|
" Launch(,CH_SPACE);"
|
|||
|
"}"
|
|||
|
);
|
|||
|
|
|||
|
AutoComplete;
|
|||
|
WinBorder;
|
|||
|
WinMax;
|
|||
|
DocCursor;
|
|||
|
DocClear;
|
|||
|
dc2=DCAlias;
|
|||
|
Fs->task_end_cb=&TaskEndCB;
|
|||
|
|
|||
|
ode=ODENew(STATE_NUM,1e-6,ODEF_HAS_MASSES);
|
|||
|
ode->derive=&MyDerivative;
|
|||
|
ode->drag_v2=0.002;
|
|||
|
ode->drag_v3=0.00001;
|
|||
|
ode->acceleration_limit=5e3;
|
|||
|
|
|||
|
// ode->t_scale=0.1; //Uncomment this to go in slow motion.
|
|||
|
|
|||
|
Init;
|
|||
|
QueIns(&m1,ode->last_mass);
|
|||
|
QueIns(&m2,ode->last_mass);
|
|||
|
QueIns(&m3,ode->last_mass);
|
|||
|
QueIns(&s,ode->last_spring);
|
|||
|
|
|||
|
QueIns(ode,Fs->last_ode);
|
|||
|
|
|||
|
Fs->draw_it=&DrawIt;
|
|||
|
|
|||
|
try {
|
|||
|
GetKey;
|
|||
|
blast_off=TRUE;
|
|||
|
while (TRUE) {
|
|||
|
switch (GetChar(,FALSE)) {
|
|||
|
case '\n':
|
|||
|
Init;
|
|||
|
GetKey;
|
|||
|
blast_off=TRUE;
|
|||
|
break;
|
|||
|
case CH_SHIFT_ESC:
|
|||
|
case CH_ESC:
|
|||
|
goto rs_done;
|
|||
|
}
|
|||
|
}
|
|||
|
rs_done:
|
|||
|
} catch
|
|||
|
PutExcept;
|
|||
|
QueRem(ode);
|
|||
|
ODEDel(ode);
|
|||
|
DocClear;
|
|||
|
SettingsPop;
|
|||
|
DCFill;
|
|||
|
DCDel(dc2);
|
|||
|
MenuPop;
|
|||
|
}
|
|||
|
|
|||
|
RocketScience;
|
|||
|
|