LaserModel::LaserModel(mapgrid * * map)
{
this->map = map;
this->range_cov = 0.10 * 0.10;
this->range_bad = 0.50;
PreCompute();
this->range_count = 0;
this->ranges = (laser_range_t *)calloc(LASER_MAX_RANGES, sizeof(laser_range_t));
};
void HermiteRBF::BuildLeftHandMatix(vector<Point_3> InterpoPoints,vector<vector<double>>& LeftHandMatrix)
{
//compute CubicBasisFunc,gradient and hessian between each pair of points
vector<vector<double>> CBF;
vector<vector<GeneralMatrix>> GradCBF;
vector<vector<GeneralMatrix>> HesCBF;
PreCompute(InterpoPoints,CBF,GradCBF,HesCBF);
vector<double> ParaRow0,ParaRow1,ParaRow2,ParaRow3;//extra constraints on parameters
//build left hand matrix
for (unsigned int i=0;i<InterpoPoints.size();i++)
{
vector<double> FmGraRow;//f(i,j)-grad f(i,j)
vector<double> GramHesRow[3];//grad f(i,j)-hessian f(i,j)
for (unsigned int j=0;j<InterpoPoints.size();j++)
{
//f(i,j)-grad f(i,j)
FmGraRow.push_back(CBF.at(i).at(j));
FmGraRow.push_back(-GradCBF.at(i).at(j)[0][0]);
FmGraRow.push_back(-GradCBF.at(i).at(j)[1][0]);
FmGraRow.push_back(-GradCBF.at(i).at(j)[2][0]);
//grad f(i,j)-hessian f(i,j)
for (int k=0;k<3;k++)
{
GramHesRow[k].push_back(GradCBF.at(i).at(j)[k][0]);
GramHesRow[k].push_back(-HesCBF.at(i).at(j)[k][0]);
GramHesRow[k].push_back(-HesCBF.at(i).at(j)[k][1]);
GramHesRow[k].push_back(-HesCBF.at(i).at(j)[k][2]);
}
}
//polynomial part
FmGraRow.push_back(InterpoPoints.at(i).x());
FmGraRow.push_back(InterpoPoints.at(i).y());
FmGraRow.push_back(InterpoPoints.at(i).z());
FmGraRow.push_back(1);
GramHesRow[0].push_back(1);GramHesRow[0].push_back(0);GramHesRow[0].push_back(0);GramHesRow[0].push_back(0);
GramHesRow[1].push_back(0);GramHesRow[1].push_back(1);GramHesRow[1].push_back(0);GramHesRow[1].push_back(0);
GramHesRow[2].push_back(0);GramHesRow[2].push_back(0);GramHesRow[2].push_back(1);GramHesRow[2].push_back(0);
//extra parameter rows
ParaRow0.push_back(1);ParaRow0.push_back(0);ParaRow0.push_back(0);ParaRow0.push_back(0);
ParaRow1.push_back(InterpoPoints.at(i).x());
ParaRow1.push_back(1);
ParaRow1.push_back(0);
ParaRow1.push_back(0);
ParaRow2.push_back(InterpoPoints.at(i).y());
ParaRow2.push_back(0);
ParaRow2.push_back(1);
ParaRow2.push_back(0);
ParaRow3.push_back(InterpoPoints.at(i).z());
ParaRow3.push_back(0);
ParaRow3.push_back(0);
ParaRow3.push_back(1);
LeftHandMatrix.push_back(FmGraRow);
LeftHandMatrix.push_back(GramHesRow[0]);
LeftHandMatrix.push_back(GramHesRow[1]);
LeftHandMatrix.push_back(GramHesRow[2]);
}
for (int i=0;i<4;i++)
{
ParaRow0.push_back(0);
ParaRow1.push_back(0);
ParaRow2.push_back(0);
ParaRow3.push_back(0);
}
LeftHandMatrix.push_back(ParaRow0);
LeftHandMatrix.push_back(ParaRow1);
LeftHandMatrix.push_back(ParaRow2);
LeftHandMatrix.push_back(ParaRow3);
}
// [X,isValid] = rayTrace(D,C,vertices,faces);
//
// Inputs:
// D - 3xM matrix of direction vectors (single)
// C - 3x1 vector for camera center (single)
// vertices - 3xN matrix of vertices (single)
// faces - 3xK matrix of triangle face indices (int32)
//
// Outputs:
// X - 3xM matrix of 3D points that intersects the mesh (single)
// isValid - 1xM vector that indicates whether 3D point is valid intersection (logic)
void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
if(nrhs != 4) {
mexErrMsgTxt("Error: 4 args. needed.");
return;
}
// Get inputs:
float *D = (float*)mxGetData(prhs[0]);
float *C = (float*)mxGetData(prhs[1]);
float *vertices = (float*)mxGetData(prhs[2]);
int *faces = (int*)mxGetData(prhs[3]);
int Nrays = mxGetN(prhs[0]);
int Nvertices = mxGetN(prhs[2]);
int Nfaces = mxGetN(prhs[3]);
fprintf(stdout,"Nfaces: %d\n",Nfaces);
fflush(stdout);
// Allocate outputs:
plhs[0] = mxCreateNumericMatrix(3,Nrays,mxSINGLE_CLASS,mxREAL);
float *X = (float*)mxGetData(plhs[0]);
plhs[1] = mxCreateLogicalMatrix(1,Nrays);
bool *isValid = (bool*)mxGetData(plhs[1]);
plhs[2] = mxCreateNumericMatrix(1,Nrays,mxINT32_CLASS,mxREAL);
int *faceNdx = (int*)mxGetData(plhs[2]);
// Allocate temporary memory:
float *lambda_num = (float*)mxMalloc(Nfaces*sizeof(float));
int *Uaxis = (int*)mxMalloc(Nfaces*sizeof(int));
int *Vaxis = (int*)mxMalloc(Nfaces*sizeof(int));
int *Waxis = (int*)mxMalloc(Nfaces*sizeof(int));
float *Cu = (float*)mxMalloc(Nfaces*sizeof(float));
float *Cv = (float*)mxMalloc(Nfaces*sizeof(float));
float *bnu = (float*)mxMalloc(Nfaces*sizeof(float));
float *bnv = (float*)mxMalloc(Nfaces*sizeof(float));
float *cnu = (float*)mxMalloc(Nfaces*sizeof(float));
float *cnv = (float*)mxMalloc(Nfaces*sizeof(float));
float *vu = (float*)mxMalloc(Nfaces*sizeof(float));
float *vv = (float*)mxMalloc(Nfaces*sizeof(float));
float *nu = (float*)mxMalloc(Nfaces*sizeof(float));
float *nv = (float*)mxMalloc(Nfaces*sizeof(float));
// Pre-compute quantities on mesh:
PreCompute(vertices,faces,Nfaces,C,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv);
// Compute KD-tree:
fprintf(stdout,"Creating KDtree...\n");
fflush(stdout);
int maxDepth = 20;//3;
float bound_min[3] = {INF,INF,INF};
float bound_max[3] = {-INF,-INF,-INF};
int *leaves = (int*)mxMalloc(Nfaces*sizeof(int));
for(int i = 0; i < Nfaces; i++) leaves[i] = i;
for(int i = 0; i < Nvertices; i++) {
for(int j = 0; j < 3; j++) {
if(vertices[j+i*3]<bound_min[j]) bound_min[j] = vertices[j+i*3];
if(vertices[j+i*3]>bound_max[j]) bound_max[j] = vertices[j+i*3];
}
}
// for(int i = 0; i < 3; i++) fprintf(stdout,"Bounds %d: %f %f\n",i,bound_min[i],bound_max[i]);
KDtree *root = CreateKDtree(vertices,faces,bound_min,bound_max,0,leaves,Nfaces,0,maxDepth);
// mxFree(leaves);
fprintf(stdout,"Finished creating KDtree...\n");
fflush(stdout);
int Nleaves = Nfaces;
leaves = (int*)mxMalloc(Nleaves*sizeof(int));
for(int i = 0; i < Nleaves; i++) leaves[i] = i;
// Get 3D points:
float maxLambda;
float min_lambda,max_lambda;
// float *Di;
float Di[3];
int j;
float ll;
int jmax;
for(int i = 0; i < Nrays; i++) {
// Di = D+i*3;
// This was originally written for LH coordinates; change for RH coordinates
Di[0] = -D[i*3];
Di[1] = -D[1+i*3];
Di[2] = -D[2+i*3];
// Get bounds on ray:
// min_lambda = -INF;
max_lambda = -INF;
for(j = 0; j < 3; j++) {
ll = (bound_min[j]-C[j])/Di[j];
if(ll<=0) {
if((C[j]>=bound_min[j])&&(ll>max_lambda)) max_lambda = ll;
// if((C[j]<bound_min[j])&&(ll>min_lambda)) min_lambda = ll;
}
ll = (bound_max[j]-C[j])/Di[j];
if(ll<=0) {
// if((C[j]>=bound_max[j])&&(ll>min_lambda)) min_lambda = ll;
if((C[j]<bound_max[j])&&(ll>max_lambda)) max_lambda = ll;
}
}
min_lambda = 0.0f;
// fprintf(stdout,"Min/max lambda: (%f,%f)\n",min_lambda,max_lambda);
// fflush(stdout);
// Intersect triangles:
maxLambda = IntersectTriangle_KDtree(Di,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv,root,min_lambda,max_lambda,C,&jmax);
// maxLambda = IntersectTriangle(Di,Uaxis,Vaxis,Waxis,lambda_num,bnu,bnv,cnu,cnv,Cu,Cv,nu,nv,vu,vv,leaves,Nleaves);
if(maxLambda != -INF) {
isValid[i] = 1;
faceNdx[i] = jmax+1;
for(j = 0; j < 3; j++) X[j+i*3] = -D[j+i*3]*maxLambda+C[j];
}
}
mxFree(leaves);
// Free memory:
mxFree(lambda_num);
mxFree(Uaxis);
mxFree(Vaxis);
mxFree(Waxis);
mxFree(Cu);
mxFree(Cv);
mxFree(bnu);
mxFree(bnv);
mxFree(cnu);
mxFree(cnv);
mxFree(vu);
mxFree(vv);
mxFree(nu);
mxFree(nv);
// Deallocate KDtree:
FreeKDtree(root);
}
void AeroplaneEntity::Think(EventListener* const eventListener, EnvironementProvider* const environementprovider)
{
// Check input
_rotateValue = 0;
if(eventListener->GetInputLeft())
_rotateValue = 1;
if(eventListener->GetInputRight())
_rotateValue = -1;
if(eventListener->GetInputPropNum())
_propelValue = (float)eventListener->GetInputPropNumValue();
//Pre compute values
PreCompute();
// Strenghts
Compute_F_weight();
Compute_F_propel();
Compute_F_Rx();
Compute_F_Rz(eventListener->GetEllapsedTime());
// Velocity
_velocity.x += (_F_weight.x + _F_propel.x + _F_Rx.x + _F_Rz.x)*eventListener->GetEllapsedTime()/_mass;
_velocity.y -= (_F_weight.y + _F_propel.y + _F_Rx.y + _F_Rz.y)*eventListener->GetEllapsedTime()/_mass;
//Momentum offset
_rotationIncidence = GetRotation() + std::atan2f(_velocity.y, _velocity.x)*180.f/PI;
if(_rotationIncidence > 180.f) _rotationIncidence -= 360.f;
else if(_rotationIncidence < -180.f) _rotationIncidence += 360.f;
if(_vNormal >= _MVlim) _Moffset = -_rotationIncidence*_MVcoef;
else if(_vNormal <= -_MVlim) _Moffset = _rotationIncidence*_MVcoef;
else _Moffset = _rotationIncidence*ComputeMOffsetCoef(_MVcoef, _MVlim);
//Moment Elev
if(_velocity_local.x >= _MVlim) _MCelev = _MelevCoef;
else if(_velocity_local.x <= 0.f) _MCelev = 0;
else _MCelev = ComputeMElevCoef(_MelevCoef, _MVlim);
this->Rotate(_Moffset);
_vRotation += 1.f*_rotateValue*_MCelev;
_vRotation *= 0.6f;
this->Move(_velocity.x, _velocity.y);
this->Rotate(_vRotation);
//Compute global landing points
_landingPoint1_global = this->TransformToGlobal(_landingPoint1);
_landingPoint2_global = this->TransformToGlobal(_landingPoint2);
_landingPoint1_isInCollision = _landingPoint1_global.y >= 535;
_landingPoint2_isInCollision = _landingPoint2_global.y >= 535;
if(_landingPoint1_isInCollision && _landingPoint2_isInCollision)
{
float newRotation = std::atan2f(_landingPoint1.y - _landingPoint2.y, _landingPoint1.x - _landingPoint2.x);
this->SetRotation( newRotation*180.f/PI);
this->SetY( 535 - ( _landingPoint1.y - GetCenter().y )*std::cosf(newRotation) );
this->_velocity.y = 0;
}
else if(_landingPoint1_isInCollision)
{
}
else if(_landingPoint2_isInCollision)
{
}
}
