/*
* Penalizes total jerk.
*/
double total_jerk_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double* s = traj->s_coeffs;
double t = traj->T;
double s_dot[5];
differentiate(s_dot, s, 5);
double s_d_dot[4];
differentiate(s_d_dot, s_dot, 4);
double jerk[3];
differentiate(jerk, s_d_dot, 3);
double total_jerk = 0.0;
double dt = T / 100.0;
double j = 0.0;
for (uint8_t i = 0; i < 100; i++)
{
t = dt * i;
j = calculate(jerk, t, 3);
total_jerk += abs(j*dt);
}
double jerk_per_second = total_jerk / T;
cost = logistic(jerk_per_second / EXPECTED_JERK_IN_ONE_SEC );
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
/*
* Penalizes max jerk.
*/
double max_jerk_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double* s = traj->s_coeffs;
double t = traj->T;
double s_dot[5];
differentiate(s_dot, s, 5);
double s_d_dot[4];
differentiate(s_d_dot, s_dot, 4);
double jerk[3];
differentiate(jerk, s_d_dot, 3);
std::vector<double> all_jerks;
for (uint8_t i = 0; i < 100; i++)
{
all_jerks.push_back(calculate(jerk, T/100 * i, 4));
}
double max_jerk = *std::max_element(all_jerks.begin(), all_jerks.end());
if (abs(max_jerk) > MAX_JERK)
{
cost = 1.0;
}
else
{
cost = logistic(MAX_JERK/(MAX_JERK-abs(max_jerk)));
}
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
void BirdEye::updateFlightStatus()
{
double roll,pitch,yaw;
try{
listener->lookupTransform("/world", vicon,
ros::Time(0), *transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
//get angles in ZYX
tf::Matrix3x3(transform->getRotation()).getEulerYPR(yaw,pitch,roll);
//transform from euler zyx to euler zxy
transformZYXtoZXY(yaw,pitch,roll);
//getting positions x,y,z
currentPoint.x = transform->getOrigin().x()*1000;
currentPoint.y = transform->getOrigin().y()*1000;
currentPoint.z = transform->getOrigin().z()*1000;
flightLog();
//calculate velocity using one step differentiate, can be improved by moving average method.
viconLostHandle();
currentVel = differentiate(pointLog);
velLog.push_back(currentVel);
currentAcc = differentiate(velLog);
accLog.push_back(currentAcc);
//update target position
targetPosClient.call(srv_getTargetPos);
targetVelClient.call(srv_getTargetVel);
targetPos.x = srv_getTargetPos.response.x;
targetPos.y = srv_getTargetPos.response.y;
targetPos.z = srv_getTargetPos.response.z;
targetVel.x = srv_getTargetVel.response.x;
targetVel.y = srv_getTargetVel.response.y;
targetVel.z = srv_getTargetVel.response.z;
ROS_DEBUG("eye target %.3f %.3f %.3f", targetVel.x, targetVel.y, targetVel.z);
//targetPos = zeroVector;
//targetVel = zeroVector;
targetLog.push_back(targetPos);
}
int main (void)
{
int poly_degree=0;
while (poly_degree>-1)//while polynomial size is greater than -1
{
//set the size of the polynomial
printf("Please enter the maximum degree of the polynomial: ");
scanf("%d", &poly_degree);
if (poly_degree>-1) {
size=poly_degree;
//set up the memory for the values in coefficients
coefficients=new int [size+1];
signage=new char[size+1];
//now input the coefficient
input_coefficients ();
//print the polynomial
output_polynomial (POLY);
//differentiate
differentiate();
//print the diferential
output_polynomial (DIFF);
//delete assigned memory
delete [] coefficients;
delete [] signage;
}
}
return 0;
}
Polinom Polinom:: operator-- (int)
{
Polinom temp = *this;
differentiate();
return temp;
}
RicoPtr Rico::differentiate(RicoPtr This, RicoPtr Wrt) const {
// Translate generic RicoPtr to BasicRV id, if possible.
if(!Wrt->isBasicRV()) {
cout << "ERROR: Cannot differentiate with respect to non-BasicRV" << endl;
return This;
}
return differentiate(This, id());
}
RicoPtr Rico::differentiate_single(RicoPtr This) const {
set<RicoID> rv_set;
collectBasicRVs(rv_set);
if(rv_set.size() != 1)
cout << "WARNING: Rico::differentiate_single found " << rv_set.size() << " variables" << endl;
set<RicoID>::iterator ii = rv_set.begin(); // choose first one only! Ignore others.
return differentiate(This, *ii);
}
const vRicoPtr Rico::differentiate(RicoPtr This) const {
set<RicoID> rv_set;
collectBasicRVs(rv_set);
vRicoPtr drv;
for(set<RicoID>::iterator ii = rv_set.begin(); ii != rv_set.end(); ++ii) {
drv.push_back(differentiate(This, *ii));
}
return drv;
}
/*
* Tests for keratinocyte_differentiate()
*
*/
void test_differentiate_stem_z()
{
//initialise();
char_array * id_temp;
id_temp = init_char_array();
add_char(id_temp, '1');
add_keratinocyte_agent(id_temp, K_TYPE_STEM, 0, 0, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
current_xmachine = *p_xmachine;
differentiate();
CU_ASSERT_EQUAL(get_type(), K_TYPE_TA);
}
void audio2midi::execute(AudioSampleBuffer* in, MidiBuffer* out, int SR, float threshold, float offtreshold, int* nummessagessent, int mindt, float keyscaling)
{
buffersize = in->getNumSamples();
incopy = *in;
//peeks = *in;
//rmsbuf = *in;
//diff1 = *in;
//diff2 = *in;
peeks.setSize(1, buffersize, false, true, false);
rmsbuf.setSize(1, buffersize, false, true, false);
diff1.setSize(1, buffersize, false, true, false);
diff2.setSize(1, buffersize, false, true, false);
peeks.clear();
rmsbuf.clear();
diff1.clear();
diff2.clear();
bandpassfilter::setup(4, SR, note);
bandpassfilter::process(buffersize, incopy.getArrayOfChannels());
incopy.applyGain(2);
if (vorige.getNumSamples() == buffersize)
{
//onset.peeks(&buffer, &peeks, &vorige);
rms(&incopy, &rmsbuf, &vorige, 10);
differentiate(&rmsbuf, &diff1, &vorige);
differentiate(&diff1, &diff2, &vorige);
difftomidi(&diff2, &incopy, out, note, 1 , threshold, offtreshold, nummessagessent, mindt, keyscaling);
}
vorige = incopy;
}
/*
* Penalizes exceeds speed limit.
*/
double exceeds_speed_limit_cost(struct trajectory* traj, int8_t target_vehicle, struct state* delta, double T, std::map<int8_t, Vehicle>* predictions, bool verbose)
{
double cost = 0.0;
double* s = traj->s_coeffs;
double t = traj->T;
double s_dot[5];
differentiate(s_dot, s, 5);
double speed = calculate(s_dot, t, 5);
if (speed > SPEED_LIMIT/2.24)
{
cost = 1.0 + logistic((speed-SPEED_LIMIT/2.24)/(SPEED_LIMIT/2.24));;
}
else
{
cost = logistic((SPEED_LIMIT/2.24-speed)/(SPEED_LIMIT/2.24));
}
if (verbose == true)
{
printf("[COST][INFO] %24s : %f\n", __func__, cost);
}
return cost;
}
int main()
{
struct node *p1,*p2,*p3,*p4,*p5,*p6;
int x,ch;
do
{
printf("\n\t\t\t---------Polynomial----------\n");
printf("\n\n\t1.Create\n\t2.Add\n\t3.Subtract\n\t4.Multiply\n\t5.Differentiate\n\t6.Exit");
printf("\nEnter choice : ");
scanf("%d",&ch);
switch(ch)
{
case 1:
p1=(struct node*) malloc(sizeof(struct node));
p2=(struct node*) malloc(sizeof(struct node));
p1=NULL;
p2=NULL;
p1=create(p1);
x=count(p1);
sort(p1,x);
p2=create(p2);
x=count(p2);
sort(p2,x);
display(p1);
display(p2);
break;
case 2:
p3=add(p1,p2);
display(p1);
display(p2);
printf("\nSum is : \n");
display(p3);
break;
case 3:
p3=subtract(p1,p2);
display(p1);
display(p2);
printf("\nDifference is : \n");
display(p3);
break;
case 4:
p3=multiply(p1,p2);
display(p1);
display(p2);
printf("\nProduct is : \n");
display(p3);
break;
case 5:
p3=differentiate(p1);
display(p1);
printf("\nDifferential is : \n");
display(p3);
p3=differentiate(p2);
display(p2);
printf("\nDifferential is : \n");
display(p3);
break;
case 6:
return 0;
}
}while(5);
}
int
main(int argc, char **argv)
{
cwp_String mode; /* display: real, imag, amp, arg */
int imode=AMP; /* integer abbrev. for mode in switch */
register complex *ct; /* complex trace */
int nt; /* number of points on input trace */
float dt; /* sample spacing */
float *data; /* array of data from each trace */
float *hdata; /* array of Hilbert transformed data */
float unwrap; /* PI/unwrap=min dphase assumed to by wrap*/
int wint; /* n time sampling to window */
cwp_Bool seismic; /* is this seismic data? */
int ntout;
/* Initialize */
initargs(argc, argv);
requestdoc(1);
/* Get info from first trace */
if (!gettr(&tr)) err("can't get first trace");
nt = tr.ns;
dt = ((double) tr.dt)/1000000.0;
ntout = nt + nt -1;
/* check to see if data type is seismic */
seismic = ISSEISMIC(tr.trid);
if (!seismic)
warn("input is not seismic data, trid=%d", tr.trid);
/* Get mode; note that imode is initialized to AMP */
if (!getparstring("mode", &mode)) mode = "amp";
if (STREQ(mode, "phase")) imode = ARG;
else if (STREQ(mode, "freq")) imode = FREQ;
else if (STREQ(mode, "bandwidth")) imode = BANDWIDTH;
else if (STREQ(mode, "normamp")) imode = NORMAMP;
else if (STREQ(mode, "freqw")) imode = FREQW;
else if (STREQ(mode, "thin")) imode = THIN;
else if (STREQ(mode, "fdenv")) imode = FENV;
else if (STREQ(mode, "sdenv")) imode = SENV;
else if (STREQ(mode, "q")) imode = Q;
else if (!STREQ(mode, "amp"))
err("unknown mode=\"%s\", see self-doc", mode);
/* getpar value of unwrap */
switch(imode) {
case FREQ:
if (!getparfloat("unwrap", &unwrap)) unwrap=1;
break;
case Q:
if (!getparfloat("unwrap", &unwrap)) unwrap=1;
break;
case FREQW:
if (!getparfloat("unwrap", &unwrap)) unwrap=1;
if (!getparint("wint", &wint)) wint=3;
break;
case THIN:
if (!getparfloat("unwrap", &unwrap)) unwrap=1;
if (!getparint("wint", &wint)) wint=3;
break;
case ARG:
if (!getparfloat("unwrap", &unwrap)) unwrap=0;
break;
}
/* allocate space for data and hilbert transformed data, cmplx trace */
data = ealloc1float(nt);
hdata = ealloc1float(nt);
ct = ealloc1complex(nt);
/* Loop over traces */
do {
register int i;
/* Get data from trace */
for (i = 0; i < nt; ++i) data[i] = tr.data[i];
/* construct quadrature trace with hilbert transform */
hilbert(nt, data, hdata);
/* build the complex trace */
for (i = 0; i < nt; ++i) ct[i] = cmplx(data[i],hdata[i]);
/* Form absolute value, phase, or frequency */
switch(imode) {
case AMP:
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
tr.data[i] = sqrt(re*re + im*im);
}
/* set trace id */
tr.trid = ENVELOPE;
break;
case ARG:
{
float *phase = ealloc1float(nt);
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
if (re*re+im*im) phase[i] = atan2(im, re);
else phase[i] = 0.0;
}
/* phase unwrapping */
/* default unwrap=0 for this mode */
if (unwrap!=0) unwrap_phase(nt, unwrap, phase);
/* write phase values to tr.data */
for (i = 0; i < nt; ++i) tr.data[i] = phase[i];
/* set trace id */
tr.trid = INSTPHASE;
}
break;
case FREQ:
{
float *phase = ealloc1float(nt);
float fnyq = 0.5 / dt;
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
if (re*re+im*im) {
phase[i] = atan2(im, re);
} else {
phase[i] = 0.0;
}
}
/* unwrap the phase */
if (unwrap!=0) unwrap_phase(nt, unwrap, phase);
/* compute freq(t)=dphase/dt */
differentiate(nt, 2.0*PI*dt, phase);
/* correct values greater nyquist frequency */
for (i=0 ; i < nt; ++i) {
if (phase[i] > fnyq)
phase[i] = 2 * fnyq - phase[i];
}
/* write freq(t) values to tr.data */
for (i=0 ; i < nt; ++i) tr.data[i] = phase[i];
/* set trace id */
tr.trid = INSTFREQ;
}
break;
case FREQW:
{
float fnyq = 0.5 / dt;
float *freqw = ealloc1float(nt);
float *phase = ealloc1float(nt);
float *envelop = ealloc1float(nt);
float *envelop2 = ealloc1float(nt);
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
if (re*re+im*im) {
phase[i] = atan2(im, re);
} else {
phase[i] = 0.0;
}
envelop[i] = sqrt(re*re + im*im);
}
/* unwrap the phase */
if (unwrap!=0) unwrap_phase(nt, unwrap, phase);
/* compute freq(t)=dphase/dt */
differentiate(nt, 2.0*PI*dt, phase);
/* correct values greater nyquist frequency */
for (i=0 ; i < nt; ++i) {
if (phase[i] > fnyq)
phase[i] = 2 * fnyq - phase[i];
envelop2[i]=envelop[i]*phase[i];
}
twindow(nt, wint, envelop);
twindow(nt, wint, envelop2);
/* correct values greater nyquist frequency */
for (i=0 ; i < nt; ++i) {
freqw[i] = (envelop[i] == 0.0) ? 0.0 :envelop2[i]/envelop[i];
}
/* write freq(t) values to tr.data */
for (i=0 ; i < nt; ++i) tr.data[i] = freqw[i];
/* set trace id */
tr.trid = INSTFREQ;
}
break;
case THIN:
{
float fnyq = 0.5 / dt;
float *phase = ealloc1float(nt);
float *freqw = ealloc1float(nt);
float *phase2 = ealloc1float(nt);
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
if (re*re+im*im) {
phase[i] = atan2(im, re);
} else {
phase[i] = 0.0;
}
}
/* unwrap the phase */
if (unwrap!=0) unwrap_phase(nt, unwrap, phase);
/* compute freq(t)=dphase/dt */
differentiate(nt, 2.0*PI*dt, phase);
/* correct values greater nyquist frequency */
for (i=0 ; i < nt; ++i) {
if (phase[i] > fnyq)
phase[i] = 2 * fnyq - phase[i];
phase2[i]= 2 * fnyq - phase[i];
}
/* Do windowing for Average Ins . Freq over wint*/
twindow(nt, wint, phase2);
for (i=0 ; i < nt; ++i) {
freqw[i] = phase[i] - phase2[i];
/* if (abs(freqw[i]) > fnyq)
freqw[i] = 2 * fnyq - freqw[i];
*/
/* write Thin-Bed(t) values to tr.data */
tr.data[i] = freqw[i];
}
/* set trace id */
tr.trid = INSTFREQ;
}
break;
case BANDWIDTH:
{
float *envelop = ealloc1float(nt);
float *envelop2 = ealloc1float(nt);
/* Bandwidth (Barnes 1992)
|d(envelope)/dt|
band =abs |--------------|
|2 PI envelope |
*/
for (i = 0; i < nt; ++i) {
float er = ct[i].r;
float em = ct[i].i;
envelop[i] = sqrt(er*er + em*em);
envelop2[i]=sqrt(er*er + em*em);
}
differentiate(nt, dt, envelop);
for (i = 0; i < ntout; ++i) {
if (2.0*PI*envelop2[i]!=0.0) {
tr.data[i] = ABS(envelop[i]/(2.0*PI*envelop2[i]));
} else {
tr.data[i]=0.0;
}
}
tr.trid = ENVELOPE;
}
break;
case NORMAMP:
{
float phase;
float *na = ealloc1float(nt);
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
if (re*re+im*im) phase = atan2(im, re);
else phase = 0.0;
na[i] = cos(phase);
}
for (i=0 ; i < nt; ++i) tr.data[i] = na[i];
/* set trace id */
tr.trid = INSTPHASE;
}
break;
case FENV:
{
float *amp = ealloc1float(nt);
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
amp[i] = sqrt(re*re + im*im);
}
/*conv(nt, 0, envelop, nt, 0, time, ntout, 0, ouput);*/
differentiate(nt, 2.0*PI*dt, amp);
for (i=0 ; i < nt; ++i) tr.data[i] = amp[i];
/* set trace id */
tr.trid = ENVELOPE;
}
break;
case SENV:
{
float *amp = ealloc1float(nt);
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
amp[i] = sqrt(re*re + im*im);
}
differentiate(nt, 2.0*PI*dt, amp);
differentiate(nt, 2.0*PI*dt, amp);
for (i=0 ; i < nt; ++i) tr.data[i] = amp[i];
/* set trace id */
tr.trid = ENVELOPE;
}
break;
case Q:
{
float *envelop = ealloc1float(nt);
float *envelop2 = ealloc1float(nt);
float *phase = ealloc1float(nt);
float fnyq = 0.5 / dt;
/* Banswith (Barnes 1992)
-PI Freq(t) d(envelope)/dt
band = --------------------------
envelope(t)
*/
for (i = 0; i < nt; ++i) {
float re = ct[i].r;
float im = ct[i].i;
envelop[i] = sqrt(re*re + im*im);
envelop2[i]=sqrt(re*re + im*im);
if (re*re+im*im) {
phase[i] = atan2(im, re);
} else {
phase[i] = 0.0;
}
}
/* get envelope diff */
differentiate(nt, dt, envelop);
/* unwrap the phase */
if (unwrap!=0) unwrap_phase(nt, unwrap, phase);
/* compute freq(t)=dphase/dt */
differentiate(nt, 2.0*PI*dt, phase);
for (i=0 ; i < nt; ++i) {
if (phase[i] > fnyq)
phase[i] = 2 * fnyq - phase[i];
}
for (i = 0; i < ntout; ++i) {
if (envelop[i]!=0.0)
tr.data[i] = -1*PI*phase[i]*envelop2[i]/envelop[i];
else
tr.data[i]=0.0;
}
tr.trid = INSTFREQ;
}
break;
default:
err("%s: mysterious mode=\"%s\"", __LINE__, mode);
}
tr.d1 = dt; /* for graphics */
puttr(&tr);
} while (gettr(&tr));
return(CWP_Exit());
}
Polinom& Polinom:: operator-- ()
{
differentiate();
return *this;
}
* Public test 13 (public13.c)
*
* Tests differentiating a polynomial.
*/
#include <stdio.h>
#include "polynomial.h"
int main() {
Polynomial poly1= new(),s;
add_term(&poly1, 6, 4);
add_term(&poly1, -3, 3);
add_term(&poly1, 3.5, 2);
printf("poly1 is: ");
print(poly1);
printf("\n");
s= differentiate(poly1);
printf("poly1' is: ");
print(s);
printf("\n");
clear(&poly1);
clear(&s);
return 0;
}
surface::instance_t
make(const description_t& description)
{
BOOST_LOG_TRIVIAL(debug) << "Make surface sor";
const auto& points = description.points;
spline_t spline = make_spline(points.begin(), points.end());
derivations_t derivations;
rtree_t rtree;
for (std::size_t i = 0; i < spline.size(); ++i)
{
const spline_segment_t& segment = spline[i];
const polynomial5_t derivation = differentiate(std::get<2>(segment) * std::get<2>(segment));
const float delta = std::get<1>(segment) - std::get<0>(segment);
float max = std::max
(
evaluate(std::get<2>(segment), 0.0f),
evaluate(std::get<2>(segment), delta)
);
std::array<float, 5> roots;
const auto end = solve(derivation, roots.begin());
for (auto root = roots.begin(); root != end; ++root)
if (*root >= 0.0f && *root <= delta)
max = std::max
({
max,
evaluate(std::get<2>(segment), *root)
});
const box_t box
(
point_t(-max, std::get<0>(segment), -max),
point_t(+max, std::get<1>(segment), +max)
);
derivations.emplace_back(derivation);
rtree.insert(value_t(box, i));
}
/*
const box_t box = transform
(
description.transformation,
box_t// TODO: puke =>
(
vector_t {
geo::get<X>(rtree.bounds().min_corner()),
geo::get<Y>(rtree.bounds().min_corner()),
geo::get<Z>(rtree.bounds().min_corner())
},
vector_t {
geo::get<X>(rtree.bounds().max_corner()),
geo::get<Y>(rtree.bounds().max_corner()),
geo::get<Z>(rtree.bounds().max_corner())
}
)
);
BOOST_LOG_TRIVIAL(trace) << "Box: min = " << box.min_corner() << ", max = " << box.max_corner() << std::endl;
const box_t box// TODO: puke =>
(
vector_t {
geo::get<X>(rtree.bounds().min_corner()),
geo::get<Y>(rtree.bounds().min_corner()),
geo::get<Z>(rtree.bounds().min_corner())
},
vector_t {
geo::get<X>(rtree.bounds().max_corner()),
geo::get<Y>(rtree.bounds().max_corner()),
geo::get<Z>(rtree.bounds().max_corner())
}
);
BOOST_LOG_TRIVIAL(trace) << "Box: min = " << box.min_corner() << ", max = " << box.max_corner() << std::endl;
*/
model_t model
(
std::move(spline),
std::move(derivations),
std::move(rtree),
points.front()[X],
points.back()[X],
points.front()[Y],
points.back()[Y]
// box
);
return std::make_shared<instance_impl_t<model_t>>(description.transformation, std::move(model));
// return boost::make_tuple
// (
// primitive::make<model>
// (
// description.transformation,
// std::move(spline),
// std::move(derivations),
// std::move(rtree),
// points.front()[X],
// points.back()[X],
// points.front()[Y],
// points.back()[Y]
// ),
// box,
// 3 * spline.size() + 2
// );
}
