double CAGMatrix::GetAngle() const
{
if ((m_ay <= 0.0000005) && (m_ay >= -0.0000005) &&
(m_bx <= 0.0000005) && (m_bx >= -0.0000005) &&
(m_ax >= 0.0) && (m_by >= 0.0))
return 0.0;
POINT Pts[3];
Pts[0].x = 0;
Pts[0].y = 0;
Pts[1].x = 100000;
Pts[1].y = 0;
Pts[2].x = 0;
Pts[2].y = 100000;
Transform(Pts, 3);
double AngleX = Arctan(Pts[1].x - Pts[0].x, Pts[1].y - Pts[0].y);
double AngleY = Arctan(Pts[2].x - Pts[0].x, Pts[2].y - Pts[0].y);
if (AngleX > -0.1 && AngleX <= 0.1)
AngleX = 0;
if (AngleY > -0.1 && AngleY <= 0.1)
AngleY = 0;
return AngleX;
}
//
//#############################################################################
//#############################################################################
//
YawPitchRange&
YawPitchRange::operator=(const Vector3D &vector)
{
Check_Pointer(this);
Check_Object(&vector);
//
//------------------------------------------------------------------------
// See if we have a zero length vector. If so, convert it to the identity
//------------------------------------------------------------------------
//
Verify(
Vector3D::Forward.z == 1.0f && Vector3D::Left.x == 1.0f && Vector3D::Up.y == 1.0f
);
Scalar sub_range = vector.x*vector.x + vector.z*vector.z;
range = Sqrt(sub_range + vector.y*vector.y);
if (Small_Enough(range))
{
yaw = 0.0f;
pitch = 0.0f;
}
else
{
//
//---------------------------------------------------------------------
// Isolate the yaw element. If the vector is vertical, yaw will simply
// be zero. If not, the yaw will indicate counter-clockwise deviation
// from the negative Z axis
//---------------------------------------------------------------------
//
sub_range = Sqrt(sub_range);
if (Small_Enough(sub_range))
{
yaw = 0.0f;
pitch = (vector.y > 0.0f) ? -Pi_Over_2 : Pi_Over_2;
}
else
{
yaw = Arctan(vector.x, vector.z);
pitch = -Arctan(vector.y, sub_range);
}
}
return *this;
}
//--------------------------------------------------------------------------//
//--------------------------------------------------------------------------//
bool CAGMatrix::GetRotation(int& Angle) const
{
if ((m_12 <= 0.0000005) && (-m_12 <= 0.0000005) &&
(m_21 <= 0.0000005) && (-m_21 <= 0.0000005) &&
(m_11 >= 0.0) && (m_22 >= 0.0))
return false;
POINT Pts[3];
Pts[0].x = Pts[0].y = Pts[1].y = Pts[2].x = 0;
Pts[1].x = Pts[2].y = 100000;
Transform(Pts, 3);
Angle = Arctan(Pts[1].x - Pts[0].x,Pts[1].y - Pts[0].y,1,1);
int Angle2 = Arctan(Pts[2].x - Pts[0].x,Pts[2].y - Pts[0].y,1,1);
int AngleRes = Angle2 - Angle;
if (AngleRes < 0)
AngleRes += 3600;
return ((AngleRes >= 2698) && (AngleRes <= 2702));
}
UnitQuaternion&
UnitQuaternion::LogN(
const UnitQuaternion& q
)
{
Scalar theta,scale;
scale = Sqrt(q.x*q.x + q.y*q.y + q.z*q.z );
theta = Arctan(scale,q.w);
if (scale > 0.0f)
{
scale = theta/scale;
}
x = scale*q.x;
y = scale*q.y;
z = scale*q.z;
w = 0.0f;
return *this;
}
void SpinScan(uint8_t* rawData, int init)
{
RAW_DATA raw_x, raw_y;
static float buffer[101];
static int x_buffer[101];
static int y_buffer[101];
static int ptr1 = 0;
static int ptr2;
static int counter = 0;
static int sx, sy, sz, vx, vy, pos_x, pos_y;
float k;
//////////////////////////////////
static float delay_buffer_x[63];
static float delay_buffer_y[63];
static int delayptr1 = 0;
static int delayptr2;
//////////////////////////////////
static float dummy_1[ACTIVITY_WIN]; // The buffer to hold quad RMS values
static float dummy_2[ACTIVITY_WIN]; // The buffer to hold ham RMS values
static float sum_x = 0; // Variables to hold the sum of the values inside the sliding window
static float sum_y = 0;
static int ptr3 = 0; // Pointer to the place inside the buffer to add the new data
/////////////////////////////
static int PosQBuff_start[65];
static int PosQBuff_end[65];
static int PosHBuff_start[65];
static int PosHBuff_end[65];
static int PosHBuffX[65];
static int PosHBuffY[65];
static int PosQBuffVel[65];
static int PosHBuffVel[65];
static int PQ = 0, PH = 0;
float sum;
float max, min;
int i;
float f, g;
int shift_x, shift_y;
static int state = 0;
static int zero_crossing_q = 0;
static int zero_crossing_h = 0;
float angle;
static int RepCount_Q = 0;
static int averageQ_e[3];
static int current_length_copy = 0;
static int current_length_copy2;
static int v_x_prev = 0;
static int Xposition_top = 0, Xposition_bott = 0;
static int Yposition_top = 0, Yposition_bott = 0;
static int zerocross = 0;
static int downsampler = 0;
if (init == 0 && downsampler == 0)
{
raw_x.raw_data = 0;
raw_y.raw_data = 0;
raw_x.data_byte[0] = rawData[0];
raw_x.data_byte[1] = rawData[1];
if ((rawData[1] & 0x80) == 0x00) // Input data is positive, set the sign to 0
{
raw_x.data_byte[2] = 0;
raw_x.data_byte[3] = 0;
}
else // Input data is negative, set the sign to 1
{
raw_x.data_byte[2] = 0xFF;
raw_x.data_byte[3] = 0xFF;
}
raw_y.data_byte[0] = rawData[2];
raw_y.data_byte[1] = rawData[3];
if ((rawData[3] & 0x80) == 0x00) // Input data is positive, set the sign to 0
{
raw_y.data_byte[2] = 0;
raw_y.data_byte[3] = 0;
}
else // Input data is negative, set the sign to 1
{
raw_y.data_byte[2] = 0xFF;
raw_y.data_byte[3] = 0xFF;
}
delay_buffer_x[delayptr1] = raw_x.raw_data;
delay_buffer_y[delayptr1] = raw_y.raw_data;
delayptr2 = delayptr1+1;
if (delayptr2 == 13)
delayptr2 = 0;
delayptr1++;
if (delayptr1 == 13)
delayptr1 = 0;
raw_x.raw_data = delay_buffer_x[delayptr2];
raw_y.raw_data = delay_buffer_y[delayptr2];
max = -1e30;
min = 1e30;
////////////////smoothing/////////////////////
/*sum_x = sum_x - dummy_1[ptr3] + raw_x.raw_data; // Oldest value is subtracted from the sum and new value is added
dummy_1[ptr3] = raw_x.raw_data; // New value replaes the oldest value in the buffer
if (counter < 21-1)
sx = 0;
else
sx = (float)(sum_x)/21; // Outout is the average of the window
sum_y = sum_y - dummy_2[ptr3] + raw_y.raw_data; // Oldest value is subtracted from the sum and new value is added
dummy_2[ptr3] = raw_y.raw_data; // New value replaes the oldest value in the buffer
if (counter < 21-1)
sy = 0;
else
sy = (float)(sum_y)/21;
(ptr3)++; // Increment the pointers to implement the sliding effect and set them to zero if they reach the end of the buffer
if ( ptr3 == 21 )
ptr3 = 0;*/
counter++;
//f6 << sx << std::endl;
//f6_2 << sy << std::endl;
/////////////////////////////////////////////
velocityX(raw_x.raw_data, &vx, 0);
velocityY(raw_y.raw_data, &vy, 0);
if ( (counter & 0x1) != 0 || counter >= 125)
{
PositionX(vx, &pos_x, 0);
PositionY(vy, &pos_y, 0);
//X << vx << std::endl;
//Y << vy << std::endl;
/*if (Quad_Start == 1 && posptrQ_s < 27)
{
PosQBuff_start[posptrQ_s] = current_length_copy;
posptrQ_s++;
}
if (Quad_End == 1 && posptrQ_e < 27)
{
PosQBuff_end[posptrQ_e] = current_length_copy;
posptrQ_e++;
}*/
if (tbuff_quadS[tptr2] == 1)
{
PosQBuff_start[26] = current_length_copy;
}
if (tbuff_quadE[tptr2] == 1)
{
PosQBuff_end[26] = current_length_copy;
quad_detected = 1;
}
if (tbuff_hamS[tptr4] == 1)
{
PosHBuff_start[26] = current_length_copy;
}
if (tbuff_hamE[tptr4] == 1)
{
PosHBuff_end[26] = current_length_copy;
ham_detected = 1;
}
/*if (Ham_Start == 1 && posptrH_s < 27)
{
PosHBuff_start[posptrH_s] = current_length_copy;
posptrH_s++;
}
if (Ham_End == 1 && posptrH_e < 27)
{
PosHBuff_end[posptrH_e] = current_length_copy;
posptrH_e++;
}*/
switch(state)
{
case 0:
if (SegmentCad == 1)
{
state = 1;
//current_length_copy = current_length;
zerocross = 0;
}
break;
case 1:
if (v_x_prev > 0 && vx < 0)
{
current_length = 0;
current_length_copy2 = current_length_copy;
Xposition_top = pos_x;
Yposition_top = pos_y;
}
if (v_x_prev < 0 && vx > 0 && zerocross == 0)
{
zerocross = 1;
Xposition_bott = pos_x;
Yposition_bott = pos_y;
}
if (SegmentCad == 0)
{
state = 0;
/*update angle here*/
f8 << Arctan(abs(Xposition_top - Xposition_bott), abs(Yposition_top - Yposition_bott)) << std::endl;
length = current_length_copy2;
current_length_copy = current_length; //new
}
break;
}
v_x_prev = vx;
/*switch(state)
{
case 0:
if (vx < 0)
{
state = 1;
length = current_length;
zero_crossing_q = 1;
zero_crossing_h = 1;
current_length = 0;
}
break;
case 1:
if (vx > 0)
state = 0;
break;
}*/
//if (posptrQ_e == 27)
if (quad_detected == 1)
{
if (length != 0)
{
angle = (float)PosQBuff_start[26]/length*360;
if (angle >= 360)
angle = 0;
}
else
angle = 0;
f6 << angle << "\t\t";
if (length != 0)
{
angle = (float)PosQBuff_end[26]/length*360;
if (angle >= 360)
angle = 0;
}
else
angle = 0;
/*if (RepCount_Q > 2)
{
averageQ_e[0] = averageQ_e[1];
averageQ_e[1] = averageQ_e[2];
averageQ_e[2] = angle;
}
if (RepCount_Q > 4)
{
if (abs(averageQ_e[2] - averageQ_e[1]) >= 180)
averageQ_e[2] -= 360;
angle = (averageQ_e[1] + averageQ_e[2])/2;
if (angle < 0)
angle += 360;
if (abs(angle - averageQ_e[0]) >= 180)
angle -= 360;
angle = (averageQ_e[0] + angle)/2;
if (angle < 0)
angle += 360;
}
RepCount_Q++;*/
f6 << angle << std::endl;
zero_crossing_q = 0;
quad_detected = 0;
Quad_Start = 0;
Quad_End = 0;
posptrQ_e = 0;
posptrQ_s = 0;
}
//if (posptrH_e == 27)
if (ham_detected == 1)
{
if (length != 0)
{
angle = (float)PosHBuff_start[26]/length*360;
if (angle >= 360)
angle = 0;
}
else
angle = 0;
f6_2 << angle << "\t\t";
if (length != 0)
{
angle = (float)PosHBuff_end[26]/length*360;
if (angle >= 360)
angle = 0;
}
else
angle = 0;
f6_2 << angle << std::endl;
zero_crossing_h = 0;
Ham_Start = 0;
Ham_End = 0;
posptrH_e = 0;
posptrH_s = 0;
ham_detected = 0;
}
g = (float)vx*vx + (float)vy*vy;
current_length += sqrt(g);
current_length_copy += sqrt(g);
}
downsampler++;
if (downsampler == 4)
downsampler = 0;
}
else if (init == 0 && downsampler != 0)
{
downsampler++;
if (downsampler == 4)
downsampler = 0;
if (tbuff_quadS[tptr2] == 1)
{
PosQBuff_start[26] = current_length_copy;
}
if (tbuff_quadE[tptr2] == 1)
{
PosQBuff_end[26] = current_length_copy;
quad_detected = 1;
}
if (tbuff_hamS[tptr4] == 1)
{
PosHBuff_start[26] = current_length_copy;
}
if (tbuff_hamE[tptr4] == 1)
{
PosHBuff_end[26] = current_length_copy;
ham_detected = 1;
}
}
else
{
state = 0;
downsampler = 0;
current_length = 0;
current_length_copy = 0;
//counter = 0;
//velocityX(0, &i, 1);
PositionX(0, &i, 1);
//velocityY(0, &i, 1);
PositionY(0, &i, 1);
}
}
