double *FindProbabilites(int k, const int city, bool *visited, double **pheromones)
{
double *taueta = new double[n];
double sum = 0.0;
for (int i = 0; i < n; ++i)
{
if (i == city)
taueta[i] = 0.0; // Prawdopodobie�stwo przej�cia z miasta x do miasta x wynosi 0
else if (visited[i] == true)
taueta[i] = 0.0; // Prawdopodobie�stwo przej�cia z miasta x do miasta odwiedzonego wynosi 0
else
{
taueta[i] = pow(pheromones[city][i], ALPHA) * pow((1.0 / EdgeLength(city, i)), BETA); // obliczenie prawdopodobie�stwa wed�ug okre�lonego wzoru
if (taueta[i] < 0.0001)
taueta[i] = 0.0001;
else if (taueta[i] > (MaxValue() / (n * 100)))
taueta[i] = MaxValue() / (n * 100);
}
sum += taueta[i];
}
double *probs = new double[n];
for (int i = 0; i <n; ++i)
probs[i] = taueta[i] / sum;
return probs;
}
void DP(int a[])
{
int i, j, k;
for (i = 1;i <= n;i++)
sum[i] = sum[i - 1] + a[i];
for (i = 0;i <= n;i++)
for (j = 0;j <= m;j++)
{
A[i][j] = 0;
B[i][j] = -1u >> 1;
}
for (i = 1;i <= n;i++)
{
A[i][1] = B[i][1] = (sum[i] % 10 + 10) % 10;
}
A[0][0] = 1;
B[0][0] = 1;
for (j = 2;j <= m;j++)
{
for (i = j;i <= n;i++)
{
for (k = j - 1;k<i;k++)
{
{
A[i][j] = MaxValue(A[i][j], A[k][j - 1] * (((sum[i] - sum[k]) % 10 + 10) % 10));
B[i][j] = MinValue(B[i][j], B[k][j - 1] * (((sum[i] - sum[k]) % 10 + 10) % 10));
}
}
}
}
mx = MaxValue(mx, A[n][m]);
mn = MinValue(mn, B[n][m]);
}
void CurveWrapper::InitChart(PageChart &Page,TransDataTable<2> &rawData)
{
Page.GetChart().Init();
if(rawData.IsEmpty())
return;
double dMax = MaxValue();
int nType = CurveType();
int nNum = PrameNum();
if( nType==1)
{
double dParme[5];
GetPrame(dParme);
Page.GetChart().SetData(dParme,nNum,0,dMax);
}
else if(nType == 2)
{
double dX[100] = {0};
double dY[100] = {0};
int dat = rawData.Data().GetCount();
rawData.m_Data.GetData().ToDuoble(dX,0);
rawData.m_Data.GetData().ToDuoble(dY,1);
Page.GetChart().SetData(dX,dY,dat);
}
}
void FTimespan::Assign( int32 Days, int32 Hours, int32 Minutes, int32 Seconds, int32 Milliseconds )
{
int64 totalms = 1000 * (60 * 60 * 24 * (int64)Days + 60 * 60 * (int64)Hours + 60 * (int64)Minutes + (int64)Seconds) + (int64)Milliseconds;
check((totalms >= MinValue().GetTotalMilliseconds()) && (totalms <= MaxValue().GetTotalMilliseconds()));
Ticks = totalms * ETimespan::TicksPerMillisecond;
}
///////////////////////////////////////////////////////////////////////////
// Constructor
///////////////////////////////////////////////////////////////////////////
CXWindow::CXWindow(int SizeX, int SizeY)
{
display = XOpenDisplay(0);
FATAL(!display);
win = XCreateSimpleWindow(display,
DefaultRootWindow(display),
0,
0,
SizeX,
SizeY,
0, // Border width
0,
0);
{
XGCValues gcv;
gcv.graphics_exposures = False;
gc = XCreateGC(display, win, GCGraphicsExposures, &gcv);
}
screen = XDefaultScreen(display);
pvis = XDefaultVisual(display, screen);
map = XDefaultColormap(display,DefaultScreen(display));
XColor exact;
XAllocNamedColor(display, map, "white", &White, &exact);
XAllocNamedColor(display, map, "black", &Black, &exact);
XAllocNamedColor(display, map, "grey41", &DimGrey, &exact);
XAllocNamedColor(display, map, "grey82", &LightGrey, &exact);
if (pvis->red_mask && pvis->green_mask && pvis->blue_mask)
{
RMult = MultValue(pvis->red_mask);
RMax = MaxValue(pvis->red_mask);
GMult = MultValue(pvis->green_mask);
GMax = MaxValue(pvis->green_mask);
BMult = MultValue(pvis->blue_mask);
BMax = MaxValue(pvis->blue_mask);
}
else
RMult = RMax = GMult = GMax = BMult = BMax = 0;
wm_delete_window = XInternAtom(display, "WM_DELETE_WINDOW", 0);
XSetWMProtocols(display, win, &wm_delete_window, 1);
XMapRaised(display, win);
}
sf::IntRect Collision::GetAABB(const sf::Sprite& Object) {
//Get the top left corner of the sprite regardless of the sprite's center
//This is in Global Coordinates so we can put the rectangle back into the right place
sf::Vector2f pos = Object.TransformToGlobal(sf::Vector2f(0, 0));
//Store the size so we can calculate the other corners
sf::Vector2f size = Object.GetSize();
float Angle = Object.GetRotation();
//Bail out early if the sprite isn't rotated
if (Angle == 0.0f) {
return sf::IntRect(static_cast<int> (pos.x),
static_cast<int> (pos.y),
static_cast<int> (pos.x + size.x),
static_cast<int> (pos.y + size.y));
}
//Calculate the other points as vectors from (0,0)
//Imagine sf::Vector2f A(0,0); but its not necessary
//as rotation is around this point.
sf::Vector2f B(size.x, 0);
sf::Vector2f C(size.x, size.y);
sf::Vector2f D(0, size.y);
//Rotate the points to match the sprite rotation
B = RotatePoint(B, Angle);
C = RotatePoint(C, Angle);
D = RotatePoint(D, Angle);
//Round off to int and set the four corners of our Rect
int Left = static_cast<int> (MinValue(0.0f, B.x, C.x, D.x));
int Top = static_cast<int> (MinValue(0.0f, B.y, C.y, D.y));
int Right = static_cast<int> (MaxValue(0.0f, B.x, C.x, D.x));
int Bottom = static_cast<int> (MaxValue(0.0f, B.y, C.y, D.y));
//Create a Rect from out points and move it back to the correct position on the screen
sf::IntRect AABB = sf::IntRect(Left, Top, Right, Bottom);
AABB.Offset(static_cast<int> (pos.x), static_cast<int> (pos.y));
return AABB;
}
double getmaxEdgeLength(TVertex* v0,TVertex* v1,TVertex* v2,TVertex* v3)
{
double Nbrs[] = { distance( v0->pos(),v1->pos() ),
distance( v0->pos(),v2->pos() ),
distance( v0->pos(),v3->pos() ),
distance( v1->pos(),v2->pos() ),
distance( v1->pos(),v3->pos() ),
distance( v2->pos(),v3->pos() ) };
return MaxValue(Nbrs,6 );
}
bool NWChar::TryParse(const Text &t, wchar_t &c)
{
char cadena[1001];
((Text *)&t)->GetAnsiString(cadena, 1000);
errno = 0;
long long ll = atoll(cadena);
if (errno != 0) return false;
if (ll < MinValue() || ll > MaxValue()) return false;
c = ll;
return true;
}
int main ()
{
int heapSize = 12;
int intArray[4] = {4, 6, 2, 8};
heapHndl testHeap, builtHeap;
testHeap = NewHeap (heapSize);
if (IsEmpty (testHeap))
{
printf ("working\n");
}
Insert (testHeap, 9);
Insert (testHeap, 3);
Insert (testHeap, 5);
Insert (testHeap, 2);
Insert (testHeap, 32);
Insert (testHeap, 6);
Insert (testHeap, 1);
Insert (testHeap, 43);
Insert (testHeap, 23);
Insert (testHeap, 42);
Insert (testHeap, 65);
Insert (testHeap, 57);
if (IsFull (testHeap))
{
printf ("heap currently full\n");
}
DeleteMax (testHeap);
builtHeap = BuildHeap (5, intArray, 4);
printf ("%d\n", MaxValue (testHeap));
printf ("%d\n", MaxValue (builtHeap));
FreeHeap (testHeap);
FreeHeap (builtHeap);
return 0;
}
bool NUInt::TryParse(const Text &text, unsigned int &n)
{
char tt[1001];
((Text *)&text)->GetAnsiString(tt, 1000);
errno = 0;
long long ll = atoll(tt);
if (errno != 0) return false;
if (ll > MaxValue() || ll < MinValue()) return false;
n = ll;
return true;
}
void Cost(float *inpMatr,char rows,float cost,float *outKoff)
{
float infnorm;
MaxValue(inpMatr, rows, &infnorm);
if (infnorm == 0)
*outKoff = 0;
else
if ((infnorm > 0) && (infnorm < cost))
*outKoff = 1;
else
*outKoff=cost/(infnorm);
}
void Integer<T>::SetValue(value_t val)
{
const T theValue = T(val);
if (value_t(theValue) < val)
{
std::stringstream s;
s << __PRETTY_FUNCTION__ << " " << Name();
s << " : value(" << val << ") bigger than the maximum allowed value(" << MaxValue() << ")";
throw std::invalid_argument(s.str());
}
const T newVal = SwapBytes<T>(theValue);
const unsigned char* buf = reinterpret_cast<const unsigned char*>(&newVal);
memcpy(&data_[0], buf, sizeof(newVal));
}
int MinValue(Board* state, int turn, clock_t st){
int beta = 99999;
if(CutOffTest(state, st)){
return state->getValue();
}
BoardQueue *q;
q = state->successor(turn);
// cout << endl;
while(!q->isEmpty()){
Board* s = q->dequeue();
beta = Min(beta, MaxValue(s,changeTurn(turn), st));
delete s;
}
delete q;
return beta;
}
FTimespan FTimespan::FromDays( double Days )
{
check((Days >= MinValue().GetTotalDays()) && (Days <= MaxValue().GetTotalDays()));
return FTimespan(Days * ETimespan::TicksPerDay);
}
void occupancy()
{
gStyle->SetPalette(1);
TH1D *lg_samle_distribution = new TH1D("sample distribution LG", "sample distribution LG", 1024, 0, 1023);
TH1D *hg_samle_distribution = new TH1D("sample distribution HG", "sample distribution HG", 1024, 0, 1023);
TH2D *hg_samp_vs_amplitude = new TH2D("samples vs amplitude HG" , "amplitude vs samples HG", 100, 0, 99, 100, 0, 99);
TH2D *lg_samp_vs_amplitude = new TH2D("samples vs amplitude LG" , "amplitude vs samples LG", 100, 0, 99, 100, 0, 99);
TH1D *hg_ampl_distribution = new TH1D("adc amplitude distribution HG", "adc amplitude distribution HG", 1024, 0, 1023);
TH1D *lg_ampl_distribution = new TH1D("adc amplitude distribution LG", "adc amplitude distribution LG", 1024, 0, 1023);
AliHLTPHOSMapper *fMapperPtr = new AliHLTPHOSMapper();
int nev = 100;
AliAltroData altrodata;
AliAltroBunch *altrobunchPtr = new AliAltroBunch;
ifstream fin;
int length;
AliAltroDecoder *decoder = new AliAltroDecoder();
double total_hg_cnannels = 64*56*5;
double total_lg_cnannels = 64*56*5;
double occupied_hg_channels = 0;
double occupied_lg_channels = 0;
double total_hg_samples = 0;
double total_lg_samples = 0;
ifstream fin;
int length;
double datasize = 0;
double datasizecnt = 0;
bool filexists = true;
for(int evt = 0; evt < nev; evt ++)
{
// for(int ddl =1792; ddl <= 1811; ddl ++)
for(int ddl =1800; ddl <= 1803; ddl ++)
{
char filename[256];
// sprintf(filename, "/home/perthi/hlt/data/simdata/PbPbbench_b%d_b%d/raw%d/PHOS_%d.ddl", b, b, evt, ddl);
// sprintf(filename, "/home/perthi/data2/data/simdata/PbPbbench_N8000/raw%d/PHOS_%d.ddl", evt, ddl);
sprintf(filename, "/media/disk/accorde_small_scintillator/bak/run55806_1/raw%d/PHOS_%d.ddl", evt, ddl);
FILE *fp = fopen(filename, "r");
if(fp == 0)
{
cout << "evt "<< evt <<"ERROR, file pointer is zero" << endl;
// fclose(fp);
// continue;
filexists = false;
}
else
{
filexists = true;
// cout << "TP0"<< endl;
fclose(fp);
// cout << "TP1"<< endl;
fin.open(filename, ios::binary);
fin.seekg (0, ios::end);
length = fin.tellg();
datasize += length ;
datasizecnt ++;
fin.seekg (0, ios::beg);
char *dataPtr = new char[length];
fin.read (dataPtr,length);
fin.close();
decoder->SetMemory((UChar_t*)dataPtr, length);
decoder->Decode();
while(decoder->NextChannel(&altrodata) == true)
{
Int_t x = fMapperPtr->fHw2geomapPtr[altrodata.GetHadd()].fXCol;
Int_t z = fMapperPtr->fHw2geomapPtr[altrodata.GetHadd()].fZRow;
Int_t gain = fMapperPtr->fHw2geomapPtr[altrodata.GetHadd()].fGain;
int tmp = MaxValue(0, (int)altrodata.GetDataSize() -2, (int *)altrodata.GetData());
if(gain == 0)
{
if(altrodata.GetDataSize() > 0)
{
occupied_lg_channels ++;
total_lg_samples += altrodata.GetDataSize();
lg_samle_distribution->Fill(altrodata.GetDataSize());
lg_samp_vs_amplitude->Fill( altrodata.GetDataSize(), tmp);
lg_ampl_distribution->Fill(tmp);
}
}
else if(gain ==1)
{
if(altrodata.GetDataSize() > 0)
{
occupied_hg_channels ++;
total_hg_samples += altrodata.GetDataSize();
hg_samle_distribution->Fill(altrodata.GetDataSize());
hg_samp_vs_amplitude->Fill( altrodata.GetDataSize(), tmp);
hg_ampl_distribution->Fill(tmp);
}
}
}
}
}
// }
if(filexists == true)
{
cout << "event" << evt <<": ";
cout << "The occupancy of the High Gain channels for is " << 100*(occupied_hg_channels/total_hg_cnannels) <<" percent" <<endl;
cout << "The occupancy of the Low Gain channels is " << 100*(occupied_lg_channels/total_lg_cnannels) <<" percent" <<endl;
cout << "The average number of samples for high gain is " << total_hg_samples/occupied_hg_channels << endl;
cout << "The average number of samples for low gain is " << total_lg_samples/occupied_lg_channels << endl;
occupied_lg_channels = 0;
occupied_hg_channels = 0;
total_lg_samples = 0;
total_hg_samples = 0;
}
}
// }
cout << "average eventsize = " << datasize/datasizecnt << endl;
lg_samle_distribution->Draw();
TCanvas *t = new TCanvas();
t->cd();
hg_samle_distribution->Draw();
TCanvas *t2 = new TCanvas();
t2->cd();
lg_ampl_distribution->Draw();
TCanvas *t3 = new TCanvas();
t3->cd();
hg_ampl_distribution->Draw();
TCanvas *t4 = new TCanvas();
t4->cd();
hg_samp_vs_amplitude->Draw("COLZ");
TCanvas *t5 = new TCanvas();
t5->cd();
lg_samp_vs_amplitude->Draw("COLZ");
}
FTimespan FTimespan::FromSeconds( double Seconds )
{
check((Seconds >= MinValue().GetTotalSeconds()) && (Seconds <= MaxValue().GetTotalSeconds()));
return FTimespan(Seconds * ETimespan::TicksPerSecond);
}
FTimespan FTimespan::FromMinutes( double Minutes )
{
check((Minutes >= MinValue().GetTotalMinutes()) && (Minutes <= MaxValue().GetTotalMinutes()));
return FTimespan(Minutes * ETimespan::TicksPerMinute);
}
FTimespan FTimespan::FromMilliseconds( double Milliseconds )
{
check((Milliseconds >= MinValue().GetTotalMilliseconds()) && (Milliseconds <= MaxValue().GetTotalMilliseconds()));
return FTimespan(Milliseconds * ETimespan::TicksPerMillisecond);
}
inline void Set(float value) noexcept
{
float volume = std::min(std::max(MinValue(), value), MaxValue());
FMODCHECK(m_pVCA->setVolume(value));
}
void CalcAng(float4 coor0,float4 coor1,float4 coor2,float4 coor3 , double* dangle, double *cangle, double *vol,double* mxdangle)
{
double x21,y21,z21, x31,y31,z31, x41,y41,z41, x32,y32,z32, x42,y42,z42, x43,y43,z43 ,
x123,y123,z123,o123, x124,y124,z124,o124, x134,y134,z134,o134, x234,y234,z234,o234,
o21,o31,o41,o32,o42,o43;
double d[6];
double c[12];
x21 = coor1.x - coor0.x;
y21 = coor1.y - coor0.y;
z21 = coor1.z - coor0.z;
x31 = coor2.x - coor0.x;
y31 = coor2.y - coor0.y;
z31 = coor2.z - coor0.z;
x41 = coor3.x - coor0.x;
y41 = coor3.y - coor0.y;
z41 = coor3.z - coor0.z;
x32 = coor2.x - coor1.x;
y32 = coor2.y - coor1.y;
z32 = coor2.z - coor1.z;
x42 = coor3.x - coor1.x;
y42 = coor3.y - coor1.y;
z42 = coor3.z - coor1.z;
x43 = coor3.x - coor2.x;
y43 = coor3.y - coor2.y;
z43 = coor3.z - coor2.z;
x123 = y21*z31 - z21*y31;
y123 = z21*x31 - x21*z31;
z123 = x21*y31 - y21*x31;
o123 = sqrt(x123*x123 + y123*y123 + z123*z123);
x124 = y41*z21 - z41*y21;
y124 = z41*x21 - x41*z21;
z124 = x41*y21 - y41*x21;
o124 = sqrt(x124*x124 + y124*y124 + z124*z124);
x134 = y31*z41 - z31*y41;
y134 = z31*x41 - x31*z41;
z134 = x31*y41 - y31*x41;
o134 = sqrt(x134*x134 + y134*y134 + z134*z134);
x234 = y42*z32 - z42*y32;
y234 = z42*x32 - x42*z32;
z234 = x42*y32 - y42*x32;
o234 = sqrt(x234*x234 + y234*y234 + z234*z234);
d[0] = (x123*x124 + y123*y124 + z123*z124)/(o123*o124);
d[1] = (x123*x134 + y123*y134 + z123*z134)/(o123*o134);
d[2] = (x134*x124 + y134*y124 + z134*z124)/(o134*o124);
d[3] = (x123*x234 + y123*y234 + z123*z234)/(o123*o234);
d[4] = (x124*x234 + y124*y234 + z124*z234)/(o124*o234);
d[5] = (x134*x234 + y134*y234 + z134*z234)/(o134*o234);
if (d[0] < -1.0) d[0] = -1.0;
if (d[0] > 1.0) d[0] = 1.0;
if (d[1] < -1.0) d[1] = -1.0;
if (d[1] > 1.0) d[1] = 1.0;
if (d[2] < -1.0) d[2] = -1.0;
if (d[2] > 1.0) d[2] = 1.0;
if (d[3] < -1.0) d[3] = -1.0;
if (d[3] > 1.0) d[3] = 1.0;
if (d[4] < -1.0) d[4] = -1.0;
if (d[4] > 1.0) d[4] = 1.0;
if (d[5] < -1.0) d[5] = -1.0;
if (d[5] > 1.0) d[5] = 1.0;
d[0] = (PI-acos(d[0])) * 180 / PI;
d[1] = (PI-acos(d[1])) * 180 / PI;
d[2] = (PI-acos(d[2])) * 180 / PI;
d[3] = (PI-acos(d[3])) * 180 / PI;
d[4] = (PI-acos(d[4])) * 180 / PI;
d[5] = (PI-acos(d[5])) * 180 / PI;
o21 = sqrt(x21*x21 + y21*y21 + z21*z21);
o31 = sqrt(x31*x31 + y31*y31 + z31*z31);
o41 = sqrt(x41*x41 + y41*y41 + z41*z41);
o32 = sqrt(x32*x32 + y32*y32 + z32*z32);
o42 = sqrt(x42*x42 + y42*y42 + z42*z42);
o43 = sqrt(x43*x43 + y43*y43 + z43*z43);
c[0] = (x21*x31+y21*y31+z21*z31) / (o21*o31);
c[1] =-(x21*x32+y21*y32+z21*z32) / (o21*o32);
c[3] = (x21*x41+y21*y41+z21*z41) / (o21*o41);
c[4] =-(x21*x42+y21*y42+z21*z42) / (o21*o42);
c[6] = (x32*x42+y32*y42+z32*z42) / (o32*o42);
c[7] =-(x32*x43+y32*y43+z32*z43) / (o32*o43);
c[9] = (x31*x41+y31*y41+z31*z41) / (o31*o41);
c[10]=-(x31*x43+y31*y43+z31*z43) / (o31*o43);
if (c[0] < -1.0) c[0] = -1.0;
if (c[0] > 1.0) c[0] = 1.0;
if (c[1] < -1.0) c[1] = -1.0;
if (c[1] > 1.0) c[1] = 1.0;
if (c[3] < -1.0) c[3] = -1.0;
if (c[3] > 1.0) c[3] = 1.0;
if (c[4] < -1.0) c[4] = -1.0;
if (c[4] > 1.0) c[4] = 1.0;
if (c[6] < -1.0) c[6] = -1.0;
if (c[6] > 1.0) c[6] = 1.0;
if (c[7] < -1.0) c[7] = -1.0;
if (c[7] > 1.0) c[7] = 1.0;
if (c[9] < -1.0) c[9] = -1.0;
if (c[9] > 1.0) c[9] = 1.0;
if (c[10]< -1.0) c[10]= -1.0;
if (c[10]> 1.0) c[10]= 1.0;
c[0] = acos(c[0]);
c[1] = acos(c[1]);
c[2] = PI - c[0] - c[1];
c[3] = acos(c[3]);
c[4] = acos(c[4]);
c[5] = PI - c[3] - c[4];
c[6] = acos(c[6]);
c[7] = acos(c[7]);
c[8] = PI - c[6] - c[7];
c[9] = acos(c[9]);
c[10]= acos(c[10]);
c[11]= PI - c[9] - c[10];
c[0] = c[0] * 180 / PI;
c[1] = c[1] * 180 / PI;
c[2] = c[2] * 180 / PI;
c[3] = c[3] * 180 / PI;
c[4] = c[4] * 180 / PI;
c[5] = c[5] * 180 / PI;
c[6] = c[6] * 180 / PI;
c[7] = c[7] * 180 / PI;
c[8] = c[8] * 180 / PI;
c[9] = c[9] * 180 / PI;
c[10] = c[10]* 180 / PI;
c[11] = c[11]* 180 / PI;
*dangle = MinValue(d,6);
*mxdangle = MaxValue(d,6);
*cangle = MinValue(c,12);
*vol = (x123*x41 + y123*y41 + z123*z41) / 6;
};
FTimespan FTimespan::FromHours( double Hours )
{
check((Hours >= MinValue().GetTotalHours()) && (Hours <= MaxValue().GetTotalHours()));
return FTimespan(Hours * ETimespan::TicksPerHour);
}
void FunctionalRegulator(float *V_target, float *Coord_target, float *Coord_cur, float *V_out) // расчет требуемой скорости вращения двигателей
{
//float cosinus = cosf(-robotCoord[2]), sinus = sinf(-robotCoord[2]);
//float Velocity[3] = {(*(V_target)), (*(V_target+1)),*(V_target+2)};
float localVelocity[3];
//float Radian = (*(V_target+2));
float realRad = robotCoord[2];
//float Ml[4][2] = {(sinus+cosinus), (cosinus-sinus), (cosinus-sinus), -(sinus+cosinus), (cosinus-sinus), -(sinus+cosinus), (sinus+cosinus), (cosinus-sinus)};
// float Mfi[4] = {-(0.14), -(0.14),-(0.14), -(0.14)}; //матрица расчета угловой скорости
float Mrot[3][3] = {cos(realRad) , sin(realRad), 0,
-sin(realRad), cos(realRad), 0,
0, 0, 1}; //матрица пересчета глобальных скоростей в локальные
float VLine[4], VRot[4], VSum[4];
float MaxMotSpeed;
float MaxLine,MaxRot;
// float Kr[2][float InverseKinematics[4][4]2] = {1.0, 0.0, 0.0, 1.0};
//float Kfi = 1.0;
//float deltaVect[2] = {(*(Coord_target)) - (*(Coord_cur)), (*(Coord_target+1)) - (*(Coord_cur+1))};
//float deltaPhi = (*(Coord_target+2)) - (*(Coord_cur+2));
float Vvect[4], Vrad[4];
float buf1[3], buf2[4][2], buf3[4];
//float vectVelOrig[4], vectVelWeighted[4], vectErrOrig[4]/*, vectErrWeighted[3]*/;
//float phiRadOrig[4], phiRadWeighted[4], phiErrOrig[4]/*, phiErrWeighted[3]*/;
float c1;
float c2;
float Cmax;
float Kvect, Kphi, KnormVect;
Cmax = MAX_CAPACITANCE;
matrixMultiplyM2M(&Mrot[0][0], 3, 3, V_target, 3, 1, &localVelocity[0]);//Ml*Velocity speed in local coordinate system
matrixMultiplyM2M(&MLineSpeed[0][0], 4, 3, &localVelocity[0], 3, 1, &VLine[0]);//MLineSpeed*localVelocity
matrixMultiplyM2M(&MRotSpeed[0][0], 4, 3, &localVelocity[0], 3, 1, &VRot[0]);//MRotSpeed*localVelocity
matrixPlusMinus(&VLine[0], &VRot[0], 4, 1, 1, &VSum[0]);//MLineSpeed+MRotSpeed
MaxValue(&VSum[0],4, &MaxMotSpeed); //find maximum speed on wheel
if (fabs(MaxMotSpeed) > MAX_CAPACITANCE)
{
c1 = fabs(MAX_CAPACITANCE/MaxMotSpeed );
} else c1 = 1;
matrixMultiplyS2M(&VSum[0], 4, 1, c1, &V_out[0]);
int i = 0;
for(i; i < 4; i++)
{
if (curState.pidEnabled) setSpeedMaxon(WHEELS[i], V_out[i]);
}
//matrixMultiplyM2M(&Ml[0][0], 4, 2, &Kr[0][0], 2, 2, &buf2[0][0]);//Ml*Kr
//matrixMultiplyM2M(&buf2[0][0], 4, 2, &deltaVect[0], 2, 1, &vectErrOrig[0]);//*(delta_vect)
//matrixMultiplyS2M(&Mfi[0], 4, 1, Radian, &phiRadOrig[0]);//Mfi*Radian;
//matrixMultiplyS2M(&Mfi[0], 4, 1, Kfi, &buf3[0]);// Mfi*Kfi
// matrixMultiplyS2M(&buf3[0], 4, 1, deltaPhi, &phiErrOrig[0]);//*(delta_phi)
// Cost(&vectVelOrig[0], 4, c1, &Kvect); //cost(vectErr1,c1);
// matrixMultiplyS2M(&vectVelOrig[0], 4, 1, Kvect, &vectVelWeighted[0]);//vectErr=vectErr1*cost(vectErr1,c1);
//infnormvect(&vectVelWeighted[0], 3, &KnormVect);
//c2 =0.2+ fabs(c1 - KnormVect);
// c2 = Cmax - c1;
//Cost(&phiRadOrig[0], 4, c2, &Kphi); //cost(phiErr1,c2);
//matrixMultiplyS2M(&phiRadOrig[0], 4, 1, Kphi, &phiRadWeighted[0]);//phiErr=phiErr1*cost(phiErr1,c2);
//matrixPlusMinus(&vectVelWeighted[0], &vectErrOrig[0], 4, 1, 1, &buf1[0]);//vectVel1+vectErr
//matrixMultiplyS2M(&buf1[0], 4, 1, ONE_RO_COS_PHI, &Vvect[0]);//Vvect=((1/ro/cos(phi_rad))*(vectVel1+vectErr));
//matrixPlusMinus(&phiRadWeighted[0], &phiErrOrig[0], 4, 1, 1, &buf1[0]);//phiRad1+phiErr
//matrixMultiplyS2M(&buf1[0], 4, 1, ONE_RO_COS_PHI, &Vrad[0]);//((1/ro/cos(phi_rad))*(phiRad1+phiErr))
//matrixPlusMinus(&Vvect[0], &Vrad[0], 4, 1, 1, &V_out[0]);
}
// MaxValue
//======================================================================
bptr < MaxValue_vector > MaxValue::make(int count,...) {
bptr < MaxValue_vector > result(new MaxValue_vector());
va_list num_list;
va_start(num_list, count);
for (int i = 0; i < count; i++) {
const MaxValue & m = va_arg(num_list, const MaxValue);
result->push_back(m);
} va_end(num_list);
return result;
};
const MaxValue & MaxValue::Double1 = MaxValue(1.); //正規化
const MaxValue & MaxValue::Double100 = MaxValue(100.); //正規化
const MaxValue & MaxValue::Int5Bit = MaxValue(31., true, false, 5); //5bit
const MaxValue & MaxValue::Int6Bit = MaxValue(63., true, false, 6); //6bit
const MaxValue & MaxValue::Int7Bit = MaxValue(127., true, false, 7); //7bit
const MaxValue & MaxValue::Int8Bit = MaxValue(255., true, true, 8); //一般常用的RGB code
const MaxValue & MaxValue::Double255 = MaxValue(255.); //各種bit數的RGB code通用
const MaxValue & MaxValue::Int9Bit = MaxValue(510., true, true, 9); //9bit
const MaxValue & MaxValue::Double1020 = MaxValue(1020, false, true, -1); //10bit
const MaxValue & MaxValue::Int10Bit = MaxValue(1020, true, true, 10); //10bit
const MaxValue & MaxValue::Int11Bit = MaxValue(2040, true, true, 11); //11bit
const MaxValue & MaxValue::Double4080 = MaxValue(4080, false, true, -1); //12bit
const MaxValue & MaxValue::Int12Bit = MaxValue(4080, true, true, 12); //12bit
const MaxValue & MaxValue::Int13Bit = MaxValue(8160, true, true, 13); //13bit
const MaxValue & MaxValue::Int14Bit = MaxValue(16320, true, true, 14); //14bit
const MaxValue & MaxValue::Int15Bit = MaxValue(32640, true, true, 15); //15bit