//---------------------------------------------------------
bool NDG3D::StartUp3D()
//---------------------------------------------------------
{
// Purpose : Setup script, building operators, grid, metric,
// and connectivity tables for 3D meshes of tetrahedra.
// Definition of constants
Np = (N+1)*(N+2)*(N+3)/6; Nfp = (N+1)*(N+2)/2; Nfaces=4; NODETOL = 1e-7;
// Compute nodal set
DVec x1,y1,z1;
Nodes3D(N, x1,y1,z1);
xyztorst(x1,y1,z1, r,s,t);
// Build reference element matrices
V = Vandermonde3D(N,r,s,t); invV = inv(V);
MassMatrix = trans(invV)*invV;
::Dmatrices3D(N, r, s, t, V, Dr, Ds, Dt);
// build coordinates of all the nodes
IVec va = EToV(All,1), vb = EToV(All,2), vc = EToV(All,3), vd = EToV(All,4);
x = 0.5*(-(1.0+r+s+t)*VX(va) + (1.0+r)*VX(vb) + (1.0+s)*VX(vc) + (1.0+t)*VX(vd));
y = 0.5*(-(1.0+r+s+t)*VY(va) + (1.0+r)*VY(vb) + (1.0+s)*VY(vc) + (1.0+t)*VY(vd));
z = 0.5*(-(1.0+r+s+t)*VZ(va) + (1.0+r)*VZ(vb) + (1.0+s)*VZ(vc) + (1.0+t)*VZ(vd));
// find all the nodes that lie on each edge
IVec fmask1,fmask2,fmask3,fmask4;
fmask1 = find( abs(1.0+t), '<', NODETOL);
fmask2 = find( abs(1.0+s), '<', NODETOL);
fmask3 = find( abs(1.0+r+s+t), '<', NODETOL);
fmask4 = find( abs(1.0+r), '<', NODETOL);
Fmask.resize(Nfp,4); // set shape (M,N) before concat()
Fmask = concat(fmask1,fmask2,fmask3,fmask4); // load vector into shaped matrix
Fx=x(Fmask,All); Fy=y(Fmask,All); Fz=z(Fmask,All);
// Create surface integral terms
Lift3D();
// calculate geometric factors and normals
Normals3D();
Fscale = sJ.dd(J(Fmask,All));
// Build connectivity matrix
tiConnect3D(EToV, EToE, EToF);
// Build connectivity maps
BuildMaps3D();
// Compute weak operators (could be done in preprocessing to save time)
DMat Vr,Vs,Vt; GradVandermonde3D(N, r, s, t, Vr, Vs, Vt);
VVT = V*trans(V);
Drw = (V*trans(Vr))/VVT; Dsw = (V*trans(Vs))/VVT; Dtw = (V*trans(Vt))/VVT;
return true;
}
ObjExplosion::ObjExplosion(const Point3d& pos, const Point3d& vel):
Drawable3D(std::string("explosion"), pos, vel),
sparkles()
{
for(int i =0;i != 100;i ++){
double vx = VX() +rand()%1000/(double)1-50;
double vy = VY()+ rand()%1000/(double)1-50;
double vz = VZ()+ rand()%1000/(double)1-50;
std::cout << pos.z << std::endl;
sparkles.push_back(
Sparkle(pos, Point3d(vx,vy,vz))
);
}
}
void Obj3DDrawable::update(Uint32 ticks){
//todo
// setPosition(getPosition((double)ticks/10*getVelocity() ));
setPosition(
Point3d(X()+VX()/1000*ticks, Y()+VY()/1000*ticks, Z()+VZ()/1000*ticks )
);
/// std::cout << Z() << std::endl;
// std::cout << VZ()<< std::endl;
//
ticks = ticks;//delete this line
projectedLines.clear();
for(std::vector<Line3d>::iterator i = lines.begin();i != lines.end();i ++){
projectedLines. push_back(Viewpoint::getInstance().lookAtLine3D(*i));
}
}
void Block3DDrawable::update(Uint32 ticks){
// turningRight = true;
// accelerating = true;
// double currentSpeed= sqrt(VX()*VX()+VY()*VY() + VZ()*VZ());
// std::cout << currentSpeed << std::endl;
//accelerating
/* if(accelerating && currentSpeed < topSpeed){
VX(VX()+VX()*acceleration*(VX()/currentSpeed)*ticks/1000);
VY(VY()+VY()*acceleration*(VY()/currentSpeed)*ticks/1000);
VZ(VZ()+VZ()*acceleration*(VZ()/currentSpeed)*ticks/1000);
}
*/
//not accelerating
/* if(!accelerating && currentSpeed >0){
if(VX()> VX()*acceleration*(VX()/currentSpeed)*ticks/1000){
VX(VX()-VX()*acceleration*(VX()/currentSpeed)*ticks/1000);
}else{
VX(0);
}
if(VY()> VY()*acceleration*(VY()/currentSpeed)*ticks/1000){
VY(VY()-VY()*acceleration*(VY()/currentSpeed)*ticks/1000);
}else{
VY(0);
}
if(VZ()> VZ()*acceleration*(VZ()/currentSpeed)*ticks/1000){
VZ(VZ()-VZ()*acceleration*(VZ()/currentSpeed)*ticks/1000);
}else{
VZ(0);
}
}*/
/*
//turning right
if(turningRight){
// std::cout << "turning" << std::endl;
VZ(VZ()- VX()*0.1);
VX(VX()+ VZ()*0.1);
}
if(turningLeft){
// std::cout << "turning" << std::endl;
VZ(VZ()+ VX()*0.1);
VX(VX()- VZ()*0.1);
}
//turning left
*/
updateLinesPosition();
Obj3DDrawable::update(ticks);
// turningRight = false;
// turningLeft = false;
accelerating = false;
VX(0);
VY(0);
VZ(0);
}
//---------------------------------------------------------
void NDG3D::Hrefine3D(IVec& refineflag)
//---------------------------------------------------------
{
DVec lVX("lVX"), lVY("lVY"), lVZ("lVZ");
int a=1, b=2, c=3, d=4, e=5,
f=6, g=7, h=8, i=9, j=10;
// a b c d e f g h i j
lVX.load(10, " 0 1 0 0 0.5 0.5 0.0 0.0 0.5 0.0 ");
lVY.load(10, " 0 0 1 0 0.0 0.5 0.5 0.0 0.0 0.5 ");
lVZ.load(10, " 0 0 0 1 0.0 0.0 0.0 0.5 0.5 0.5 ");
assert(4 == Nfaces);
IMat lEToV(8,4, "lEToV"), im84(8,4, "im84"), oFnodes(4,6, "oFnodes");
set84(lEToV, 8,4,
a, e, g, h,
e, b, f, i,
g, f, c, j,
h, i, j, d,
e, g, h, i,
h, i, g, j,
e, f, g, i,
g, i, f, j);
set46(oFnodes, 4,6,
a, e, b, f, c, g,
a, e, b, i, d, h,
b, f, c, j, d, i,
a, g, c, j, d, h);
IMat vnum(gRowData, 4,3, "1 2 3 1 2 4 2 3 4 3 1 4");
int lK = lEToV.num_rows();
IMat lBCType(lK, Nfaces), tm1, tm2;
IVec tv(3),tv1,tv2, oFn,vnp,ksids;
int kk=0,ff=0,oo=0;
for (kk=1; kk<=lK; ++kk) {
for (ff=1; ff<=Nfaces; ++ff) {
for (oo=1; oo<=Nfaces; ++oo)
{
oFn = oFnodes.get_row(oo);
vnp = vnum.get_row(ff);
tm1 = lEToV(kk, vnp); const IVec& knodes = dynamic_cast<const IVec&>(tm1);
//tv = intersect(lEToV(k, vnum(p,All)), oFnodes(o,All));
tv = intersect(knodes, oFn);
if ( tv.length()==3 ) {
lBCType(kk, ff) = oo;
}
}
}
}
int NV = VX.length()+1;
int sp_len = NV + NV*NV;
// sparse buffers nnz vals,triplet
CSd newVX(sp_len,1, NV, 1, 1);
CSd newVY(sp_len,1, NV, 1, 1);
CSd newVZ(sp_len,1, NV, 1, 1);
Index1D II(1, NV-1);
newVX(II,1) = VX;
newVY(II,1) = VY;
newVZ(II,1) = VZ;
IVec ids("ids");
int oldK = K, f1=0;
for (int k1=1; k1<=oldK; ++k1) {
if (refineflag(k1))
{
a = EToV(k1,1);
b = EToV(k1,2);
c = EToV(k1,3);
d = EToV(k1,4);
e = NV + std::max(a*NV+b, b*NV + a);
f = NV + std::max(b*NV+c, c*NV + b);
g = NV + std::max(a*NV+c, c*NV + a);
h = NV + std::max(a*NV+d, d*NV + a);
i = NV + std::max(b*NV+d, d*NV + b);
j = NV + std::max(c*NV+d, d*NV + c);
//ks = [k1, K+1:K+7];
IVec ks(1, k1); ks.append(Range(K+1,K+7));
//--------------------------
// EToV(ks,:) = [a e g h;
// e b f i;
// g f c j;
// h i j d;
// e g h i;
// h i g j;
// e f g i;
// g i f j];
//--------------------------
set84(im84, 8,4,
a, e, g, h,
e, b, f, i,
g, f, c, j,
h, i, j, d,
e, g, h, i,
h, i, g, j,
e, f, g, i,
g, i, f, j);
//EToV(ks,All) = im84;
EToV.merge_rows(ks, im84);
for (f1=1; f1<=Nfaces; ++f1)
{
tv = lBCType(All,f1);
ids = find(tv, '!', 0);
ksids = ks(ids);
tm1 = lBCType(ids,f1);
tm2 = BCType(k1, (const IVec&)tm1);
// expand BCType to accommodate new range
int maxI=ksids.max_val(), maxR=BCType.num_rows();
if (maxI>maxR) {BCType.realloc(maxI, BCType.num_cols());}
BCType(ksids,f1) = trans(tm2);
}
K += 7;
newVX.set1(e,1, 0.5*(VX(a)+VX(b)));
newVX.set1(f,1, 0.5*(VX(b)+VX(c)));
newVX.set1(g,1, 0.5*(VX(c)+VX(a)));
newVX.set1(h,1, 0.5*(VX(a)+VX(d)));
newVX.set1(i,1, 0.5*(VX(b)+VX(d)));
newVX.set1(j,1, 0.5*(VX(c)+VX(d)));
newVY.set1(e,1, 0.5*(VY(a)+VY(b)));
newVY.set1(f,1, 0.5*(VY(b)+VY(c)));
newVY.set1(g,1, 0.5*(VY(c)+VY(a)));
newVY.set1(h,1, 0.5*(VY(a)+VY(d)));
newVY.set1(i,1, 0.5*(VY(b)+VY(d)));
newVY.set1(j,1, 0.5*(VY(c)+VY(d)));
newVZ.set1(e,1, 0.5*(VZ(a)+VZ(b)));
newVZ.set1(f,1, 0.5*(VZ(b)+VZ(c)));
newVZ.set1(g,1, 0.5*(VZ(c)+VZ(a)));
newVZ.set1(h,1, 0.5*(VZ(a)+VZ(d)));
newVZ.set1(i,1, 0.5*(VZ(b)+VZ(d)));
newVZ.set1(j,1, 0.5*(VZ(c)+VZ(d)));
}
}
//-------------------------------------
// drop duplicates and sort
//-------------------------------------
newVX.compress(true);
newVY.compress(true);
newVZ.compress(true);
//ids = sort(unique(EToV(:)), 'ascend');
ids = unique(EToV);
int len=ids.max_val();
IVec gnum(len);
gnum(ids) = Range(1,ids.length());
VX = full( newVX, ids );
VY = full( newVY, ids );
VZ = full( newVZ, ids );
// EToV = gnum(EToV);
EToV.set_map(EToV, gnum);
int NV_old = NV-1; // local counters
this->Nv = VX.length(); // update member variable
umLOG(1, "Hrefine3D [%d] : old Nv = %4d, new Nv = %4d\n", ++refine_count, NV_old, Nv);
umLOG(1, " old K = %4d, new K = %4d\n\n", oldK, K);
#if (0)
tetramesh(EToV, [VX', VY', VZ'])
#endif
}
/** Periodic function
*/
void meteo_stick_periodic(void)
{
// Read ADC
#ifdef MS_PRESSURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.pressure);
#endif
#ifdef MS_DIFF_PRESSURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.diff_pressure);
#endif
#ifdef MS_TEMPERATURE_SLAVE_IDX
ads1220_periodic(&meteo_stick.temperature);
#endif
// Read PWM
#ifdef MS_HUMIDITY_PWM_INPUT
meteo_stick.humidity_period = pwm_input_period_tics[MS_HUMIDITY_PWM_INPUT];
meteo_stick.current_humidity = get_humidity(meteo_stick.humidity_period);
#endif
#if USE_MS_EEPROM
if (meteo_stick.eeprom.data_available) {
// Extract calibration data
if (!mtostk_populate_cal_from_buffer(&meteo_stick.calib, (uint8_t *)(meteo_stick.eeprom.rx_buf + 3))) {
// Extraction failed
// Force number of calibration to 0 for all sensors
int i;
for (i = 0; i < MTOSTK_NUM_SENSORS; i++) {
meteo_stick.calib.params[i].num_temp = 0;
}
}
} else if (meteo_stick.eeprom.spi_trans.status == SPITransDone) {
// Load reading request (reading 1Kb from address 0x0)
eeprom25AA256_read(&meteo_stick.eeprom, 0x0, 1024);
}
#endif
// Log data
#if LOG_MS
if (pprzLogFile != -1) {
if (!log_ptu_started) {
#if USE_MS_EEPROM
if (meteo_stick.eeprom.data_available) {
// Print calibration data in the log header
sdLogWriteLog(pprzLogFile, "# Calibration data (UUID: %s)\n#\n", meteo_stick.calib.uuid);
int i, j, k;
for (i = 0; i < MTOSTK_NUM_SENSORS; i++) {
sdLogWriteLog(pprzLogFile, "# Sensor: %d, time: %d, num_temp: %d, num_coeff: %d\n", i,
meteo_stick.calib.params[i].timestamp,
meteo_stick.calib.params[i].num_temp,
meteo_stick.calib.params[i].num_coeff);
if (meteo_stick.calib.params[i].timestamp == 0) {
continue; // No calibration
}
for (j = 0; j < meteo_stick.calib.params[i].num_temp; j++) {
sdLogWriteLog(pprzLogFile, "# Reference temp: %.2f\n", meteo_stick.calib.params[i].temps[j]);
sdLogWriteLog(pprzLogFile, "# Coeffs:");
for (k = 0; k < meteo_stick.calib.params[i].num_coeff; k++) {
sdLogWriteLog(pprzLogFile, " %.5f", meteo_stick.calib.params[i].coeffs[j][k]);
}
sdLogWriteLog(pprzLogFile, "\n");
}
}
sdLogWriteLog(pprzLogFile, "#\n");
sdLogWriteLog(pprzLogFile,
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
log_ptu_started = TRUE;
}
#else
sdLogWriteLog(pprzLogFile,
"P(adc) T(adc) H(ticks) P_diff(adc) P(hPa) T(C) H(\%) CAS(m/s) FIX TOW(ms) WEEK Lat(1e7rad) Lon(1e7rad) HMSL(mm) GS(cm/s) course(1e7rad) VZ(cm/s)\n");
log_ptu_started = TRUE;
#endif
} else {
