void lambert_api(const mxArray *prhs[5], const mxArray *plhs[4])
{
real_T (*V1)[3];
real_T (*V2)[3];
real_T (*extremal_distances)[2];
real_T (*r1vec)[3];
real_T (*r2vec)[3];
real_T tf;
real_T m;
real_T muC;
real_T exitflag;
emlrtStack st = { NULL, /* site */
NULL, /* tls */
NULL /* prev */
};
st.tls = emlrtRootTLSGlobal;
V1 = (real_T (*)[3])mxMalloc(sizeof(real_T [3]));
V2 = (real_T (*)[3])mxMalloc(sizeof(real_T [3]));
extremal_distances = (real_T (*)[2])mxMalloc(sizeof(real_T [2]));
prhs[0] = emlrtProtectR2012b(prhs[0], 0, false, -1);
prhs[1] = emlrtProtectR2012b(prhs[1], 1, false, -1);
/* Marshall function inputs */
r1vec = emlrt_marshallIn(&st, emlrtAlias(prhs[0]), "r1vec");
r2vec = emlrt_marshallIn(&st, emlrtAlias(prhs[1]), "r2vec");
tf = c_emlrt_marshallIn(&st, emlrtAliasP(prhs[2]), "tf");
m = c_emlrt_marshallIn(&st, emlrtAliasP(prhs[3]), "m");
muC = c_emlrt_marshallIn(&st, emlrtAliasP(prhs[4]), "muC");
/* Invoke the target function */
lambert(&st, *r1vec, *r2vec, tf, m, muC, *V1, *V2, *extremal_distances,
&exitflag);
/* Marshall function outputs */
plhs[0] = emlrt_marshallOut(*V1);
plhs[1] = emlrt_marshallOut(*V2);
plhs[2] = b_emlrt_marshallOut(*extremal_distances);
plhs[3] = c_emlrt_marshallOut(exitflag);
}
void ChiselServer::FillMarkerTopicWithMeshes(visualization_msgs::Marker* marker)
{
assert(marker != nullptr);
marker->header.stamp = ros::Time::now();
marker->header.frame_id = baseTransform;
marker->ns = "mesh";
marker->type = visualization_msgs::Marker::TRIANGLE_LIST;
marker->id = 0;
marker->scale.x = 1;
marker->scale.y = 1;
marker->scale.z = 1;
marker->pose.position.x = 0;
marker->pose.position.y = 0;
marker->pose.position.z = 0;
marker->pose.orientation.x = 0.0;
marker->pose.orientation.y = 0.0;
marker->pose.orientation.z = 0.0;
marker->pose.orientation.w = 1.0;
marker->color.r = 1.0f;
marker->color.g = 1.0f;
marker->color.b = 1.0f;
marker->color.a = 1.0f;
const chisel::MeshMap& meshMap = chiselMap->GetChunkManager().GetAllMeshes();
if(meshMap.size() == 0)
{
return;
}
chisel::Vec3 lightDir(0.2, 0.2, 1.0f);
lightDir.normalize();
const chisel::Vec3 ambient(0.2f, 0.2f, 0.3f);
for (const std::pair<chisel::ChunkID, chisel::MeshPtr>& meshes : meshMap)
{
const chisel::MeshPtr& mesh = meshes.second;
geometry_msgs::Point pt;
std_msgs::ColorRGBA color;
for (size_t i = 0; i < mesh->vertices.size(); i++)
{
const chisel::Vec3& vec = mesh->vertices[i];
pt.x = vec.x();
pt.y = vec.y();
pt.z = vec.z();
marker->points.push_back(pt);
if(mesh->HasColors())
{
const chisel::Vec3& meshCol = mesh->colors[i];
color.r = meshCol.x();
color.g = meshCol.y();
color.b = meshCol.z();
color.a = 1.0f;
marker->colors.push_back(color);
}
else if(mesh->HasNormals())
{
chisel::Vec3 lambert(0.6f, 0.6f, 0.5f);
const chisel::Vec3 normal = mesh->normals[i];
lambert = fmin(fmax(normal.dot(lightDir), 0), 1.0f) * lambert + ambient;
color.r = fmax(fmin(0.5f * (normal.x() + 1.0f), 1.0f), 0);
color.g = fmax(fmin(0.5f * (normal.y() + 1.0f), 1.0f), 0);
color.b = fmax(fmin(0.5f * (normal.z() + 1.0f), 1.0f), 0);
color.a = 1.0f;
marker->colors.push_back(color);
}
else
{
float zval = vec.z() - 5;
color.r = (zval) / 10.0f;
color.g = 10.0f / (fabs(zval) + 0.01f);
color.b = (zval * zval) / 10.0f;
color.a = 1.0f;
marker->colors.push_back(color);
}
}
}
}
main()
{
int step,left;
double xnclim,h,a,gm,pi,degra,xnp,prederr,xisp1,xisp2,xmf1,xmf2;
double wpay,delv,delv1,delv2,amax1,amax2,xkickdeg,top2,bot2;
double wp2,ws2,wtot2,trst2,tb2,top1,bot1,wp1,ws1,wtot,trst1,tb1;
double xlongmdeg,xlongtdeg,altnmtic,altnmmic,tf,altt,altm;
double s,scount,xlongm,xlongt,xm,ym,xt,yt,xfirstt,yfirstt;
double x1t,y1t,t,xtf,ytf,x1m,y1m,rtm1,rtm2;
double rtm,vtm1,vtm2,vc,tgo,xoldt,yoldt,x1oldt,y1oldt;
double xoldm,yoldm,x1oldm,y1oldm,delvold,xdt,ydt,x1dt,y1dt;
double xdm,ydm,x1dm,y1dm,delvd,xnmt,ynmt,xnmm,ynmm;
double altnmt,distnmt,altnmm,wgt,trst,at,vel,axt,ayt;
double tembott,atplos,xlam,xlamd,xnc,am1,am2;
double tembotm,degrad,tgolam,vrxm,vrym,distnmm,xncg,atplosg;
FILE *fptr;
fptr=fopen("DATFIL.TXT","w");
left=0;
xnclim=644.;
h=.01;
a=2.0926e7;
gm=1.4077e16;
pi=3.14159;
degrad=360./(2.*pi);
xnp=3.;
prederr=0.;
xisp1=250.;
xisp2=250.;
xmf1=.85;
xmf2=.85;
wpay=100.;
delv=20000.;
delv1=.3333*delv;
delv2=.6667*delv;
amax1=20.;
amax2=20.;
xkickdeg=80.;
top2=wpay*(exp(delv2/(xisp2*32.2))-1.);
bot2=1/xmf2-((1.-xmf2)/xmf2)*exp(delv2/(xisp2*32.2));
wp2=top2/bot2;
ws2=wp2*(1-xmf2)/xmf2;
wtot2=wp2+ws2+wpay;
trst2=amax2*(wpay+ws2);
tb2=xisp2*wp2/trst2;
top1=wtot2*(exp(delv1/(xisp1*32.2))-1.);
bot1=1/xmf1-((1.-xmf1)/xmf1)*exp(delv1/(xisp1*32.2));
wp1=top1/bot1;
ws1=wp1*(1-xmf1)/xmf1;
wtot=wp1+ws1+wtot2;
trst1=amax1*(wtot2+ws1);
tb1=xisp1*wp1/trst1;
xlongmdeg=85.;
xlongtdeg=90.;
altnmtic=0.;
altnmmic=0.;
tf=50.;
altt=altnmtic*6076.;
altm=altnmmic*6076.;
s=0.;
scount=0.;
xlongm=xlongmdeg/degrad;
xlongt=xlongtdeg/degrad;
xm=(a+altm)*cos(xlongm);
ym=(a+altm)*sin(xlongm);
xt=(a+altt)*cos(xlongt);
yt=(a+altt)*sin(xlongt);
xfirstt=xt;
yfirstt=yt;
if (left==1){
x1t=cos(pi/2.-xkickdeg/degrad+xlongt);
y1t=sin(pi/2.-xkickdeg/degrad+xlongt);
}
else{
x1t=cos(-pi/2.+xkickdeg/degrad+xlongt);
y1t=sin(-pi/2.+xkickdeg/degrad+xlongt);
}
xfirstt=xt;
yfirstt=yt;
t=0.;
predict (tf,xt,yt,x1t,y1t,&xtf,&ytf,wp1,wtot,tb1,
trst1,tb2,wp2,wtot2,trst2,wpay);
ytf=ytf+prederr;
lambert(xm,ym,tf,xtf,ytf,&vrxm,&vrym,xlongm,xlongt);
x1m=vrxm;
y1m=vrym;
rtm1=xt-xm;
rtm2=yt-ym;
rtm=sqrt(rtm1*rtm1+rtm2*rtm2);
vtm1=x1t-x1m;
vtm2=y1t-y1m;
vc=-(rtm1*vtm1+rtm2*vtm2)/rtm;
delv=0.;
L10: if(vc<0.)goto L999;
tgo=rtm/vc;
if(tgo>.1)
h=.01;
else
h=.0001;
xoldt=xt;
yoldt=yt;
x1oldt=x1t;
y1oldt=y1t;
xoldm=xm;
yoldm=ym;
x1oldm=x1m;
y1oldm=y1m;
delvold=delv;
step=1;
goto L200;
L66: step=2;
xt=xt+h*xdt;
yt=yt+h*ydt;
x1t=x1t+h*x1dt;
y1t=y1t+h*y1dt;
xm=xm+h*xdm;
ym=ym+h*ydm;
x1m=x1m+h*x1dm;
y1m=y1m+h*y1dm;
delv=delv+h*delvd;
t=t+h;
goto L200;
L55: xt=(xoldt+xt)/2+.5*h*xdt;
yt=(yoldt+yt)/2+.5*h*ydt;
x1t=(x1oldt+x1t)/2+.5*h*x1dt;
y1t=(y1oldt+y1t)/2+.5*h*y1dt;
xm=(xoldm+xm)/2+.5*h*xdm;
ym=(yoldm+ym)/2+.5*h*ydm;
x1m=(x1oldm+x1m)/2+.5*h*x1dm;
y1m=(y1oldm+y1m)/2+.5*h*y1dm;
delv=(delvold+delv)/2.+.5*h*delvd;
altt=sqrt(xt*xt+yt*yt)-a;
altm=sqrt(xm*xm+ym*ym)-a;
s=s+h;
scount=scount+h;
if(scount<.99999)goto L10;
scount=0.;
xnmt=xt/6076.;
ynmt=yt/6076.;
xnmm=xm/6076.;
ynmm=ym/6076.;
altnmt=altt/6076.;
distance(xt,yt,xfirstt,yfirstt,&distnmt);
altnmm=altm/6076.;
distance(xm,ym,xfirstt,yfirstt,&distnmm);
xncg=xnc/32.2;
atplosg=atplos/32.2;
printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distnmt
,altnmt,distnmm,altnmm,xncg,delv,atplosg);
fprintf(fptr,"%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distnmt
,altnmt,distnmm,altnmm,xncg,delv,atplosg);
goto L10;
L200:
if(t<tb1){
wgt=-wp1*t/tb1+wtot;
trst=trst1;
}
else if(t<(tb1+tb2)){
wgt=-wp2*t/tb2+wtot2+wp2*tb1/tb2;
trst=trst2;
}
else{
wgt=wpay;
trst=0.;
}
at=32.2*trst/wgt;
vel=sqrt(x1t*x1t+y1t*y1t);
axt=at*x1t/vel;
ayt=at*y1t/vel;
tembott=pow((xt*xt+yt*yt),1.5);
x1dt=-gm*xt/tembott+axt;
y1dt=-gm*yt/tembott+ayt;
xdt=x1t;
ydt=y1t;
rtm1=xt-xm;
rtm2=yt-ym;
rtm=sqrt(rtm1*rtm1+rtm2*rtm2);
vtm1=x1t-x1m;
vtm2=y1t-y1m;
vc=-(rtm1*vtm1+rtm2*vtm2)/rtm;
tgo=rtm/vc;
xlam=atan2(rtm2,rtm1);
xlamd=(rtm1*vtm2-rtm2*vtm1)/(rtm*rtm);
atplos=y1dt*cos(xlam)-x1dt*sin(xlam);
if(t>25.)
xnc=xnp*vc*xlamd;
else
xnc=0.;
if(xnc>xnclim)xnc=xnclim;
if(xnc<-xnclim)xnc=-xnclim;
delvd=fabs(xnc);
am1=-xnc*sin(xlam);
am2=xnc*cos(xlam);
tembotm=pow((xm*xm+ym*ym),1.5);
x1dm=-gm*xm/tembotm+am1;
y1dm=-gm*ym/tembotm+am2;
xdm=x1m;
ydm=y1m;
if(step<=1)
goto L66;
else
goto L55;
L999:
xnmt=xt/6076.;
ynmt=yt/6076.;
xnmm=xm/6076.;
ynmm=ym/6076.;
altnmt=altt/6076.;
distance(xt,yt,xfirstt,yfirstt,&distnmt);
altnmm=altm/6076.;
distance(xm,ym,xfirstt,yfirstt,&distnmm);
xncg=xnc/32.2;
atplosg=atplos/32.2;
printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distnmt
,altnmt,distnmm,altnmm,xncg,delv,atplosg);
fprintf(fptr,"%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distnmt
,altnmt,distnmm,altnmm,xncg,delv,atplosg);
printf("%10.3f %10.3f %10.3f\n",t,rtm,delv);
fclose (fptr);
return 0;
}
