Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
//
//#############################################################################
//#############################################################################
//
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;
}
Ejemplo n.º 3
0
//--------------------------------------------------------------------------//
//--------------------------------------------------------------------------//
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));
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 5
0
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);
		
	}
}