//gamma is useless but I keep it for simmetry
	//with Euler_direction
	void Euler_direction2angles(Matrix1D<DOUBLE> &v0,
		DOUBLE &alpha, DOUBLE &beta)
	{
		DOUBLE abs_ca, sb, cb;
		DOUBLE aux_alpha;
		DOUBLE aux_beta;
		DOUBLE error, newerror;
		Matrix1D<DOUBLE> v_aux;
		Matrix1D<DOUBLE> v;

		//if not normalized do it so
		v.resize(3);
		v = v0;
		v.selfNormalize();

		v_aux.resize(3);
		cb = v(2);

		if (fabs((cb)) > 0.999847695)/*one degree */
		{
			std::cerr << "\nWARNING: Routine Euler_direction2angles is not reliable\n"
				"for small tilt angles. Up to 0.001 deg it should be OK\n"
				"for most applications but you never know";
		}

		if (fabs((cb - 1.)) < FLT_EPSILON)
		{
			alpha = 0.;
			beta = 0.;
		}
		else
		{/*1*/

			aux_beta = acos(cb); /* beta between 0 and PI */


			sb = sin(aux_beta);

			abs_ca = fabs(v(0)) / sb;
			if (fabs((abs_ca - 1.)) < FLT_EPSILON)
				aux_alpha = 0.;
			else
				aux_alpha = acos(abs_ca);

			v_aux(0) = sin(aux_beta) * cos(aux_alpha);
			v_aux(1) = sin(aux_beta) * sin(aux_alpha);
			v_aux(2) = cos(aux_beta);

			error = fabs(dotProduct(v, v_aux) - 1.);
			alpha = aux_alpha;
			beta = aux_beta;

			v_aux(0) = sin(aux_beta) * cos(-1. * aux_alpha);
			v_aux(1) = sin(aux_beta) * sin(-1. * aux_alpha);
			v_aux(2) = cos(aux_beta);
			newerror = fabs(dotProduct(v, v_aux) - 1.);
			if (error > newerror)
			{
				alpha = -1. * aux_alpha;
				beta = aux_beta;
				error = newerror;
			}

			v_aux(0) = sin(-aux_beta) * cos(-1. * aux_alpha);
			v_aux(1) = sin(-aux_beta) * sin(-1. * aux_alpha);
			v_aux(2) = cos(-aux_beta);
			newerror = fabs(dotProduct(v, v_aux) - 1.);
			if (error > newerror)
			{
				alpha = -1. * aux_alpha;
				beta = -1. * aux_beta;
				error = newerror;
			}

			v_aux(0) = sin(-aux_beta) * cos(aux_alpha);
			v_aux(1) = sin(-aux_beta) * sin(aux_alpha);
			v_aux(2) = cos(-aux_beta);
			newerror = fabs(dotProduct(v, v_aux) - 1.);

			if (error > newerror)
			{
				alpha = aux_alpha;
				beta = -1. * aux_beta;
				error = newerror;
			}
		}/*else 1 end*/
		beta = RAD2DEG(beta);
		alpha = RAD2DEG(alpha);
	}/*Eulerdirection2angles end*/

/**
 * @brief execute autocruise
 */
void plan_run_AutoCruise()
{
    PositionStateData positionState;

    PositionStateGet(&positionState);
    PathDesiredData pathDesired;
    PathDesiredGet(&pathDesired);
    FlightModeSettingsPositionHoldOffsetData offset;
    FlightModeSettingsPositionHoldOffsetGet(&offset);

    float controlVector[4];
    ManualControlCommandRollGet(&controlVector[0]);
    ManualControlCommandPitchGet(&controlVector[1]);
    ManualControlCommandYawGet(&controlVector[2]);
    controlVector[3] = 0.5f; // dummy, thrust is normalized separately
    normalizeDeadband(controlVector); // return value ignored
    ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity
    controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction

    // normalize old desired movement vector
    float vector[3] = { pathDesired.End.North - hold_position[0],
                        pathDesired.End.East - hold_position[1],
                        pathDesired.End.Down - hold_position[2] };
    float length    = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]);
    if (length < 1e-9f) {
        length = 1.0f; // should not happen since initialized properly in setup()
    }
    vector[0] /= length;
    vector[1] /= length;
    vector[2] /= length;

    // start position is advanced according to actual movement - in the direction of desired vector only
    // projection using scalar product
    float kp = (positionState.North - hold_position[0]) * vector[0]
               + (positionState.East - hold_position[1]) * vector[1]
               + (positionState.Down - hold_position[2]) * vector[2];
    if (kp > 0.0f) {
        hold_position[0] += kp * vector[0];
        hold_position[1] += kp * vector[1];
        hold_position[2] += kp * vector[2];
    }

    // new angle is equal to old angle plus offset depending on yaw input and time
    // (controlVector is normalized with a deadband, change is zero within deadband)
    float angle = RAD2DEG(atan2f(vector[1], vector[0]));
    float dT    = PIOS_DELTATIME_GetAverageSeconds(&actimeval);
    angle    += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings

    // resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0]
    vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
    vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
    vector[2] = -controlVector[1] * offset.Vertical * controlVector[3];

    pathDesired.End.North   = hold_position[0] + vector[0];
    pathDesired.End.East    = hold_position[1] + vector[1];
    pathDesired.End.Down    = hold_position[2] + vector[2];
    // start position has the same offset as in position hold
    pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
    pathDesired.Start.East  = pathDesired.End.East;
    pathDesired.Start.Down  = pathDesired.End.Down;
    PathDesiredSet(&pathDesired);
}

//.........这里部分代码省略.........
			if (allowedTransitionFlags & (1<<eTT_DirectionChange))
			{
				if (
						((oldSample.pseudoSpeed == AISPEED_RUN) || (oldSample.pseudoSpeed == AISPEED_SPRINT)) &&
						(runningDuration > minimumRunningDurationForJuke) &&
						(entitySpeed2D >= minimumSpeedForJuke)
					)
				{
					if (gEnv->pAISystem && !gEnv->pAISystem->GetSmartObjectManager()->CheckSmartObjectStates(player.GetEntity(), "Busy"))
					{
						// === IMMEDIATE DIRECTIONCHANGE ===

						// ---------------------------------
						// Assume a directionchange after moving forward for one meter (assumedDistanceToJuke=1)
						// Look up a transition math for that proposed directionchange
						// ---------------------------------
						m_pseudoSpeed = oldSample.pseudoSpeed;
						m_transitionDistance = -1;

						float assumedDistanceToJuke = 1.0f;
						CRY_ASSERT(assumedDistanceToJuke > FLT_EPSILON);

						Vec3 vToProposedMoveTarget = newSample.moveDirection * distToMoveTarget; // vector from current position to current movetarget
						Vec3 vToProposedJukePoint = oldSample.moveDirection * assumedDistanceToJuke;
						Vec3 vAfterProposedJukePoint = (vToProposedMoveTarget - vToProposedJukePoint).GetNormalizedSafe(newSample.moveDirection);

						m_jukeAngle = Ang3::CreateRadZ( vToProposedJukePoint, vAfterProposedJukePoint );
						m_transitionType = eTT_DirectionChange;
						m_bPredicted = false;
						m_future.vMoveDirection = vAfterProposedJukePoint;
						Vec3 realTargetBodyDirection = bHasLockedBodyTarget ? targetBodyDirection : vAfterProposedJukePoint;
						m_targetTravelAngle = Ang3::CreateRadZ( realTargetBodyDirection, vAfterProposedJukePoint );

						MovementTransitionsLog("[%x] Considering angle %+3.2f", gEnv->pRenderer->GetFrameID(), RAD2DEG(m_jukeAngle));

						const STransition* pTransition = NULL;
						int index = -1;
						STransitionMatch bestMatch;
						transitions.FindBestMatch(*this, &pTransition, &index, &bestMatch);

						if (pTransition)
						{
							// -------------------------------------------------------
							// We found a transition matching our guess. Adjust juke point to match the distance of the transition we found
							// -------------------------------------------------------
							float proposedTransitionDistance = (pTransition->minDistance + pTransition->maxDistance)*0.5f;
							CRY_ASSERT(proposedTransitionDistance > FLT_EPSILON);
							vToProposedJukePoint = oldSample.moveDirection * proposedTransitionDistance;
							vAfterProposedJukePoint = vToProposedMoveTarget - vToProposedJukePoint;
							float proposedDistAfterMoveTarget = vAfterProposedJukePoint.GetLength2D();
							vAfterProposedJukePoint.NormalizeSafe(newSample.moveDirection);

							if (proposedDistAfterMoveTarget >= transitions.GetMinDistanceAfterDirectionChange())
							{
								m_jukeAngle = Ang3::CreateRadZ( vToProposedJukePoint, vAfterProposedJukePoint );
								m_future.vMoveDirection = vAfterProposedJukePoint;
								realTargetBodyDirection = bHasLockedBodyTarget ? targetBodyDirection : vAfterProposedJukePoint;
								m_targetTravelAngle = Ang3::CreateRadZ( realTargetBodyDirection, vAfterProposedJukePoint );

								MovementTransitionsLog("[%x] Proposing angle %+3.2f", gEnv->pRenderer->GetFrameID(), RAD2DEG(m_jukeAngle));

								m_transitionDistance = proposedTransitionDistance;
								m_future.qOrientation = Quat::CreateRotationVDir( realTargetBodyDirection );
							}
							else
							{

    void PathPlanningWidget::insertRow(const tf::Transform& point_pos,const int count)
    {
      /*! Whenever we have a new Way-Point insereted either from the RViz or the RQT Widget the the TreeView needs to update the information and insert new row that corresponds to the new insered point.
          This function takes care of parsing the data recieved from the RViz or the RQT widget and creating new row with the appropriate data format and Children. One for the position giving us the current position of the Way-Point in all the axis.
          One child for the orientation giving us the Euler Angles of each axis.
      */

      ROS_INFO("inserting new row in the TreeView");
      QAbstractItemModel *model = ui_.treeView->model();

      //convert the quartenion to roll pitch yaw angle
      tf::Vector3 p = point_pos.getOrigin();
      tfScalar rx,ry,rz;
      point_pos.getBasis().getRPY(rx,ry,rz,1);

      if(count == 0)
      {
        model->insertRow(count,model->index(count, 0));

        model->setData(model->index(0,0,QModelIndex()),QVariant("add_point_button"),Qt::EditRole);
        pointRange();
      }
      else
      {
      //ROS_INFO_STREAM("Quartenion at add_row: "<<orientation.x()<<"; "<<orientation.y()<<"; "<<orientation.z()<<"; "<<orientation.w()<<";");

       if(!model->insertRow(count,model->index(count, 0)))  //&& count==0
       {
         return;
       }
      //set the strings of each axis of the position
      QString pos_x = QString::number(p.x());
      QString pos_y = QString::number(p.y());
      QString pos_z = QString::number(p.z());

      //repeat that with the orientation
      QString orient_x = QString::number(RAD2DEG(rx));
      QString orient_y = QString::number(RAD2DEG(ry));
      QString orient_z = QString::number(RAD2DEG(rz));

      model->setData(model->index(count,0),QVariant(count),Qt::EditRole);

      //add a child to the last inserted item. First add children in the treeview that
      //are just telling the user that if he expands them he can see details about the position and orientation of each point
      QModelIndex ind = model->index(count, 0);
      model->insertRows(0, 2, ind);
      QModelIndex chldind_pos = model->index(0, 0, ind);
      QModelIndex chldind_orient = model->index(1, 0, ind);
      model->setData(chldind_pos, QVariant("Position"), Qt::EditRole);
      model->setData(chldind_orient, QVariant("Orientation"), Qt::EditRole);
//*****************************Set the children for the position**********************************************************
      //now add information about each child separately. For the position we have coordinates for X,Y,Z axis.
      //therefore we add 3 rows of information
      model->insertRows(0, 3, chldind_pos);

      //next we set up the data for each of these columns. First the names
      model->setData(model->index(0, 0, chldind_pos), QVariant("X:"), Qt::EditRole);
      model->setData(model->index(1, 0, chldind_pos), QVariant("Y:"), Qt::EditRole);
      model->setData(model->index(2, 0, chldind_pos), QVariant("Z:"), Qt::EditRole);

      //second we add the current position information, for each position axis separately
      model->setData(model->index(0, 1, chldind_pos), QVariant(pos_x), Qt::EditRole);
      model->setData(model->index(1, 1, chldind_pos), QVariant(pos_y), Qt::EditRole);
      model->setData(model->index(2, 1, chldind_pos), QVariant(pos_z), Qt::EditRole);
//***************************************************************************************************************************

//*****************************Set the children for the orientation**********************************************************
      //now we repeat everything again,similar as the position for adding the children for the orientation
      model->insertRows(0, 3, chldind_orient);
      //next we set up the data for each of these columns. First the names
      model->setData(model->index(0, 0, chldind_orient), QVariant("Rx:"), Qt::EditRole);
      model->setData(model->index(1, 0, chldind_orient), QVariant("Ry:"), Qt::EditRole);
      model->setData(model->index(2, 0, chldind_orient), QVariant("Rz:"), Qt::EditRole);

      //second we add the current position information, for each position axis separately
      model->setData(model->index(0, 2, chldind_orient), QVariant(orient_x), Qt::EditRole);
      model->setData(model->index(1, 2, chldind_orient), QVariant(orient_y), Qt::EditRole);
      model->setData(model->index(2, 2, chldind_orient), QVariant(orient_z), Qt::EditRole);
//****************************************************************************************************************************
      pointRange();
    }

    }

void ThresholdTwists(int num_images, ModelMap &models, 
                     std::vector<ImageData> &image_data, bool panos_only) 
{
    int *num_large_twists = new int[num_images];
    int *degree = new int[num_images];

    for (int i = 0; i < num_images; i++) {
        num_large_twists[i] = 0;
        degree[i] = 0;
    }

    for (int i = 0; i < num_images; i++) {
        ModelTable::iterator iter;
        for (iter = models.Begin(i); iter != models.End(i); iter++) {
            unsigned int j = iter->first; // iter->m_index;
            
            if (i >= j)
                continue;
            
            MatchIndex idx = GetMatchIndex(i, j);
            if (models.Contains(idx)) {
                TwoFrameModel &m = models.GetModel(idx);
                
                /* Compute the twist */
                double Rp_i[9], Rp_j[9];
                matrix_transpose(3, 3, m.m_camera0.R, Rp_i);
                matrix_transpose(3, 3, m.m_camera1.R, Rp_j);
                
                double Rp_ij[9];
                matrix_transpose_product(3, 3, 3, 3, Rp_i, Rp_j, Rp_ij);
                
                double twist_angle = GetTwist(Rp_ij);
                
                if (fabs(RAD2DEG(twist_angle)) >= 12.0) { 
                    num_large_twists[i]++;
                    num_large_twists[j]++;
                }
                
                degree[i]++;
                degree[j]++;
            }
        }
    }
    
    for (int i = 0; i < num_images; i++) {
        if (degree[i] == 0)
            continue;

        double perc_large_twists = (double) num_large_twists[i] / degree[i];
        
        int w = image_data[i].GetWidth();
        int h = image_data[i].GetHeight();
        
        double ratio = (double) w / h;

        if ((panos_only || perc_large_twists < 0.4) && 
             ratio > 0.4 && ratio < 2.5) {
            continue;
        }

        printf("[ThresholdTwists] Removing image %d with score %0.3f, %0.3f\n",
               i, perc_large_twists, ratio);

        std::list<unsigned int> nbrs = models.GetNeighbors(i);
        std::list<unsigned int>::iterator iter;
        for (iter = nbrs.begin(); iter != nbrs.end(); iter++) {
            unsigned int j = *iter; // iter->m_index;
            
            if (i < j) {            
                MatchIndex idx = GetMatchIndex(i, j);
                if (models.Contains(idx)) {
                    models.RemoveModel(idx);
                }
            } else {
                MatchIndex idx = GetMatchIndex(j, i);
                if (models.Contains(idx)) {
                    models.RemoveModel(idx);
                }
            }
        }
    }
}

//.........这里部分代码省略.........
	lnk[2].d = atof(parameter.GetValue("UpperArmLength").c_str());	//상완길이
	lnk[2].a = 0;
	lnk[2].alpha = -M_PI_2;
	//4
	lnk[3].d = 0;
	lnk[3].a = 0;
	lnk[3].alpha = M_PI_2;
	//5
	lnk[4].d = atof(parameter.GetValue("LowerArmLength").c_str());	//하완길이
	lnk[4].a = 0;
	lnk[4].alpha = -M_PI_2;
	//6
	lnk[5].d = 0;
	lnk[5].a = 0;
	lnk[5].alpha = M_PI_2;
	//7
	lnk[6].d = atof(parameter.GetValue("ToolLength").c_str());		//그리퍼(손)길이
	lnk[6].a = 0;
	lnk[6].alpha = 0;


	MSLMatrix NowRot(3,3);
	MSLVector NowPos(3);

	MSLMatrix Rot(3,3);
	MSLVector Pos(3);

	double ct,st,ca,sa;
	int i;


	lnk[0].theta = DEG2RAD(m_joint[0]); // + lnk[0].joint_offset;
	ct = cos(lnk[0].theta);
	st = sin(lnk[0].theta);
	ca = cos(lnk[0].alpha);
	sa = sin(lnk[0].alpha);

	Rot(0,0) =  ct;
	Rot(1,0) =  st;
	Rot(2,0) =  0.0;
	Rot(0,1) = -ca*st;
	Rot(1,1) =  ca*ct;
	Rot(2,1) =  sa;
	Rot(0,2) =  sa*st;
	Rot(1,2) = -sa*ct;
	Rot(2,2) =  ca;
	Pos[0]   =  lnk[0].a * ct;
	Pos[1]   =  lnk[0].a * st;
	Pos[2]   =  lnk[0].d;


	for(i = 1 ; i < 7 ; i++)
	{	

		lnk[i].theta = DEG2RAD(m_joint[i]);//+ lnk[i].joint_offset;
		ct = cos(lnk[i].theta);
		st = sin(lnk[i].theta);
		ca = cos(lnk[i].alpha);
		sa = sin(lnk[i].alpha);

		NowRot(0,0) =  ct;
		NowRot(1,0) =  st;
		NowRot(2,0) =  0.0;
		NowRot(0,1) = -ca*st;
		NowRot(1,1) =  ca*ct;
		NowRot(2,1) =  sa;
		NowRot(0,2) =  sa*st;
		NowRot(1,2) = -sa*ct;
		NowRot(2,2) =  ca;
		NowPos[0]    =  lnk[i].a * ct;
		NowPos[1]    =  lnk[i].a * st;
		NowPos[2]    =  lnk[i].d;

		Pos = Pos + Rot*NowPos;
		Rot = Rot*NowRot;
	}


	objectPosition.x = Pos[0];
	objectPosition.y = Pos[1];
	objectPosition.z = Pos[2];


	//오리엔테이션에서 오일러ZYX(Z회전->Y회전->X회전) 회전의 각도를 뽑아냄
	if (Rot(2,0)==1) {
		objectPosition.roll = RAD2DEG(atan2(-Rot(0,1),-Rot(0,2)));	//alpha(x회전Roll각도)
		objectPosition.pitch = -90.;	//-PI/2;						//betta(y회전Pitch각도)
		objectPosition.yaw = 0.0;						//gamma(Z회전Yaw각도)
	} else if (Rot(2,0)==-1) {
		objectPosition.roll  = RAD2DEG(atan2(Rot(0,1),Rot(0,2)));
		objectPosition.pitch = 90.;	//PI/2;
		objectPosition.yaw = 0.0;
	} else {
		objectPosition.roll = RAD2DEG(atan2(Rot(2,1), Rot(2,2)));
		objectPosition.pitch = RAD2DEG(atan2(-Rot(2,0), sqrt(Rot(0,0)*Rot(0,0) + Rot(1,0)*Rot(1,0))));
		objectPosition.yaw = RAD2DEG(atan2(Rot(1,0), Rot(0,0)));
	}

	return objectPosition;
}

//.........这里部分代码省略.........
		p[0] = (float) collData->point2[0];
		p[1] = (float) collData->point2[1];
	} else {
		car = (tCar*) obj1;
		nsign = 1.0f;
		p[0] = (float) collData->point1[0];
		p[1] = (float) collData->point1[1];
	}

	sgVec2 n;		// Collision normal delivered by solid, corrected such that it points away from the wall.
	n[0] = nsign * (float) collData->normal[0];
	n[1] = nsign * (float) collData->normal[1];
	float pdist = sgLengthVec2(n);	// Distance of collision points.
	sgNormaliseVec2(n);

	sgVec2 r;
	sgSubVec2(r, p, (const float*)&(car->statGC));

	tCarElt *carElt = car->carElt;

	sgVec2 vp;		// Speed of car collision point in global frame of reference.
	sgVec2 rg;		// raduis oriented in global coordinates, still relative to CG (rotated aroung CG).

	float sina = sin(carElt->_yaw);
	float cosa = cos(carElt->_yaw);
	rg[0] = r[0]*cosa - r[1]*sina;
	rg[1] = r[0]*sina + r[1]*cosa;

	vp[0] = car->DynGCg.vel.x - car->DynGCg.vel.az * rg[1];
	vp[1] = car->DynGCg.vel.y + car->DynGCg.vel.az * rg[0];

	sgVec2 tmpv;
	static const float CAR_MIN_MOVEMENT = 0.02f;
	static const float CAR_MAX_MOVEMENT = 0.05f;
	sgScaleVec2(tmpv, n, MIN(MAX(pdist, CAR_MIN_MOVEMENT), CAR_MAX_MOVEMENT));
	if (car->blocked == 0) {
		sgAddVec2((float*)&(car->DynGCg.pos), tmpv);
		car->blocked = 1;
    }

	// Doing no dammage and correction if the cars are moving out of each other.
	if (sgScalarProductVec2(vp, n) > 0) {
		return;
	}

	float rp = sgScalarProductVec2(rg, n);

	// Pesudo cross product to find out if we are left or right.
	// TODO: SIGN, scrap value?
	float rpsign = n[0]*rg[1] - n[1]*rg[0];

	const float e = 1.0f;	// energy restitution
	float j = -(1.0f + e) * sgScalarProductVec2(vp, n) / (car->Minv + rp * rp * car->Iinv.z);
	const float ROT_K = 0.5f;

	// Damage.
	tdble damFactor, atmp;
	atmp = atan2(r[1], r[0]);
	if (fabs(atmp) < (PI / 3.0)) {
		// Front collision gives more damage.
		damFactor = 1.5f;
	} else {
		// Rear collision gives less damage.
		damFactor = 1.0f;
	}

	static const float DMGFACTOR = 0.00002f;
	if ((car->carElt->_state & RM_CAR_STATE_FINISH) == 0) {
		car->dammage += (int)(CAR_DAMMAGE * (DMGFACTOR*j*j) * damFactor * simDammageFactor[car->carElt->_skillLevel]);
	}

	sgScaleVec2(tmpv, n, j * car->Minv);
	sgVec2 v2a;

	if (car->collision & SEM_COLLISION_CAR) {
		sgAddVec2(v2a, (const float*)&(car->VelColl.x), tmpv);
		car->VelColl.az = car->VelColl.az + j * rp * rpsign * car->Iinv.z * ROT_K;
	} else {
		sgAddVec2(v2a, (const float*)&(car->DynGCg.vel), tmpv);
		car->VelColl.az = car->DynGCg.vel.az + j * rp * rpsign * car->Iinv.z * ROT_K;
	}

	static float VELMAX = 3.0f;
	if (fabs(car->VelColl.az) > VELMAX) {
		car->VelColl.az = SIGN(car->VelColl.az) * VELMAX;
	}

	sgCopyVec2((float*)&(car->VelColl.x), v2a);

	// Move the car for the collision lib.
	sgMakeCoordMat4(carElt->pub.posMat, car->DynGCg.pos.x, car->DynGCg.pos.y,
					car->DynGCg.pos.z - carElt->_statGC_z, RAD2DEG(carElt->_yaw),
					RAD2DEG(carElt->_roll), RAD2DEG(carElt->_pitch));
	dtSelectObject(car);
	dtLoadIdentity();
	dtTranslate(-carElt->_statGC_x, -carElt->_statGC_y, 0.0f);
	dtMultMatrixf((const float *)(carElt->_posMat));

	car->collision |= SEM_COLLISION_CAR;
}

void CCameraOffsetMgr::Update()
{
	// Testing only...
	ProcessTestingVars();
	// End Testing only...


	// Reset offsets...

	m_vPitchYawRollDelta.Init();
	m_vPosDelta.Init();

    float fTimeDelta = g_pGameClientShell->GetFrameTime();

    int i;
    for (i=0; i < MAX_CAMERA_DELTAS; i++)
	{
		if (m_CameraDeltas[i].GetTotalDelta() > 0.0f)
		{
			m_CameraDeltas[i].Pitch.UpdateVar(fTimeDelta);
			m_vPitchYawRollDelta.x += m_CameraDeltas[i].Pitch.GetValue();

			m_CameraDeltas[i].Yaw.UpdateVar(fTimeDelta);
			m_vPitchYawRollDelta.y += m_CameraDeltas[i].Yaw.GetValue();

			m_CameraDeltas[i].Roll.UpdateVar(fTimeDelta);
			m_vPitchYawRollDelta.z += m_CameraDeltas[i].Roll.GetValue();

			m_CameraDeltas[i].PosX.UpdateVar(fTimeDelta);
			m_vPosDelta.x += m_CameraDeltas[i].PosX.GetValue();

			m_CameraDeltas[i].PosY.UpdateVar(fTimeDelta);
			m_vPosDelta.y += m_CameraDeltas[i].PosY.GetValue();

			m_CameraDeltas[i].PosZ.UpdateVar(fTimeDelta);
			m_vPosDelta.z += m_CameraDeltas[i].PosZ.GetValue();
		}
	}

	for (i=0; i < MAX_STATIC_CAMERA_DELTAS; i++)
	{
		if (m_StaticCameraDeltas[i].GetTotalDelta() > 0.0f)
		{
			m_StaticCameraDeltas[i].Pitch.UpdateVar(fTimeDelta);
			m_vPitchYawRollDelta.x += m_StaticCameraDeltas[i].Pitch.GetValue();

			m_StaticCameraDeltas[i].Yaw.UpdateVar(fTimeDelta);
			m_vPitchYawRollDelta.y += m_StaticCameraDeltas[i].Yaw.GetValue();

			m_StaticCameraDeltas[i].Roll.UpdateVar(fTimeDelta);
			m_vPitchYawRollDelta.z += m_StaticCameraDeltas[i].Roll.GetValue();

			m_StaticCameraDeltas[i].PosX.UpdateVar(fTimeDelta);
			m_vPosDelta.x += m_StaticCameraDeltas[i].PosX.GetValue();

			m_StaticCameraDeltas[i].PosY.UpdateVar(fTimeDelta);
			m_vPosDelta.y += m_StaticCameraDeltas[i].PosY.GetValue();

			m_StaticCameraDeltas[i].PosZ.UpdateVar(fTimeDelta);
			m_vPosDelta.z += m_StaticCameraDeltas[i].PosZ.GetValue();
		}
	}

	ValidateDeltas();


	// Print out our current values...

	if (g_vtCamInfo.GetFloat())
	{
		if (m_vPitchYawRollDelta.x != 0.0f)
			g_pLTClient->CPrint("COM Pitch = %.4f (in Deg = %.2f)", m_vPitchYawRollDelta.x, RAD2DEG(m_vPitchYawRollDelta.x));
		if (m_vPitchYawRollDelta.y != 0.0f)
			g_pLTClient->CPrint("COM Yaw   = %.4f (in Deg = %.2f)", m_vPitchYawRollDelta.y, RAD2DEG(m_vPitchYawRollDelta.y));
		if (m_vPitchYawRollDelta.z != 0.0f)
			g_pLTClient->CPrint("COM Roll  = %.4f (in Deg = %.2f)", m_vPitchYawRollDelta.z, RAD2DEG(m_vPitchYawRollDelta.z));

		if (m_vPosDelta.x != 0.0f)
			g_pLTClient->CPrint("COM Offset X = %.2f", m_vPosDelta.x);
		if (m_vPosDelta.y != 0.0f)
			g_pLTClient->CPrint("COM Offset Y = %.2f", m_vPosDelta.y);
		if (m_vPosDelta.z != 0.0f)
			g_pLTClient->CPrint("COM Offset Z = %.2f", m_vPosDelta.z);
	}
}

void OVR_CalculateState(vr_param_t *state)
{
	vr_param_t ovrState;
	float ovrScale = vr_ovr_supersample->value;
	int eye = 0;

	for (eye = 0; eye < 2; eye++) {
		ovrDistortionMesh meshData;
		ovr_vert_t *mesh = NULL;
		ovr_vert_t *v = NULL;
		ovrDistortionVertex *ov = NULL;
		unsigned int i = 0;
		float vignette_factor;
		if (vr_ovr_maxfov->value)
		{
			renderInfo[eye].eyeFov = hmd->MaxEyeFov[eye];
		} else
		{
			renderInfo[eye].eyeFov = hmd->DefaultEyeFov[eye];
		}

		ovrState.eyeFBO[eye] = &renderInfo[eye].eyeFBO;

		ovrState.renderParams[eye].projection.x.scale = 2.0f / ( renderInfo[eye].eyeFov.LeftTan + renderInfo[eye].eyeFov.RightTan );
		ovrState.renderParams[eye].projection.x.offset = ( renderInfo[eye].eyeFov.LeftTan - renderInfo[eye].eyeFov.RightTan ) * ovrState.renderParams[eye].projection.x.scale * 0.5f;
		ovrState.renderParams[eye].projection.y.scale = 2.0f / ( renderInfo[eye].eyeFov.UpTan + renderInfo[eye].eyeFov.DownTan );
		ovrState.renderParams[eye].projection.y.offset = ( renderInfo[eye].eyeFov.UpTan - renderInfo[eye].eyeFov.DownTan ) * ovrState.renderParams[eye].projection.y.scale * 0.5f;

		// set up rendering info
		eyeDesc[eye] = ovrHmd_GetRenderDesc(hmd,(ovrEyeType) eye,renderInfo[eye].eyeFov);

		VectorSet(ovrState.renderParams[eye].viewOffset,
			-eyeDesc[eye].HmdToEyeViewOffset.x,
			eyeDesc[eye].HmdToEyeViewOffset.y,
			eyeDesc[eye].HmdToEyeViewOffset.z);

		ovrHmd_CreateDistortionMesh(hmd, eyeDesc[eye].Eye, eyeDesc[eye].Fov, ovrDistortionCap_Chromatic | ovrDistortionCap_SRGB | ovrDistortionCap_TimeWarp | ovrDistortionCap_Vignette, &meshData);

		mesh = (ovr_vert_t *) Z_TagMalloc(sizeof(ovr_vert_t) * meshData.VertexCount, TAG_RENDERER);
		v = mesh;
		ov = meshData.pVertexData; 
		for (i = 0; i < meshData.VertexCount; i++)
		{

			// DK2 display not rotated - rotate the coordinates manually
			if (vid.width < vid.height) {
				v->pos.x = -ov->ScreenPosNDC.y;
				v->pos.y = ov->ScreenPosNDC.x;
			} else {
				v->pos.x = ov->ScreenPosNDC.x;
				v->pos.y = ov->ScreenPosNDC.y;
			}

			v->texR = (*(ovrVector2f*)&ov->TanEyeAnglesR); 
			v->texG = (*(ovrVector2f*)&ov->TanEyeAnglesG);
			v->texB = (*(ovrVector2f*)&ov->TanEyeAnglesB); 
			vignette_factor = ov->VignetteFactor;
			if (vignette_factor < 0) vignette_factor = 0;
			v->color[0] = v->color[1] = v->color[2] = (GLubyte)(vignette_factor  * 255.99f);
			v->color[3] = (GLubyte)( ov->TimeWarpFactor * 255.99f );
			v++; ov++;
		}

		R_BindIVBO(&renderInfo[eye].eye,NULL,0);
		R_VertexData(&renderInfo[eye].eye,sizeof(ovr_vert_t) * meshData.VertexCount, mesh);
		R_IndexData(&renderInfo[eye].eye,GL_TRIANGLES,GL_UNSIGNED_SHORT,meshData.IndexCount,sizeof(uint16_t) * meshData.IndexCount,meshData.pIndexData);
		R_ReleaseIVBO();
		Z_Free(mesh);
		ovrHmd_DestroyDistortionMesh( &meshData );
	}
	{
		// calculate this to give the engine a rough idea of the fov
		float combinedTanHalfFovHorizontal = max ( max ( renderInfo[0].eyeFov.LeftTan, renderInfo[0].eyeFov.RightTan ), max ( renderInfo[1].eyeFov.LeftTan, renderInfo[1].eyeFov.RightTan ) );
		float combinedTanHalfFovVertical = max ( max ( renderInfo[0].eyeFov.UpTan, renderInfo[0].eyeFov.DownTan ), max ( renderInfo[1].eyeFov.UpTan, renderInfo[1].eyeFov.DownTan ) );
		float horizontalFullFovInRadians = 2.0f * atanf ( combinedTanHalfFovHorizontal ); 
		float fovX = RAD2DEG(horizontalFullFovInRadians);
		float fovY = RAD2DEG(2.0 * atanf(combinedTanHalfFovVertical));
		ovrState.aspect = combinedTanHalfFovHorizontal / combinedTanHalfFovVertical;
		ovrState.viewFovY = fovY;
		ovrState.viewFovX = fovX;
		ovrState.pixelScale = ovrScale * vid.width / (float) hmd->Resolution.w;
	}

	*state = ovrState;
}

//-----------------------------------------------------------------------------
bool CAI_PlaneSolver::GenerateCircleObstacleSuggestions( const AILocalMoveGoal_t &moveGoal, float probeDist )
{
	bool result = false;
	Vector npcLoc = m_pNpc->WorldSpaceCenter();
	Vector mins, maxs;

	m_pNpc->CollisionProp()->WorldSpaceSurroundingBounds( &mins, &maxs );
	float radiusNpc = (mins.AsVector2D() - maxs.AsVector2D()).Length() * 0.5;
	
	for ( int i = 0; i < m_Obstacles.Count(); i++ )
	{
		CBaseEntity *pObstacleEntity = NULL;

		float zDistTooFar;
		if ( m_Obstacles[i].hEntity && m_Obstacles[i].hEntity->CollisionProp() )
		{
			pObstacleEntity = m_Obstacles[i].hEntity.Get();

			if( pObstacleEntity == moveGoal.pMoveTarget && (pObstacleEntity->IsNPC() || pObstacleEntity->IsPlayer()) )
			{
				// HEY! I'm trying to avoid the very thing I'm trying to get to. This will make we wobble like a drunk as I approach. Don't do it.
				continue;
			}

			pObstacleEntity->CollisionProp()->WorldSpaceSurroundingBounds( &mins, &maxs );
			zDistTooFar = ( maxs.z - mins.z ) * 0.5 + GetNpc()->GetHullHeight() * 0.5;
		}
		else
			zDistTooFar = GetNpc()->GetHullHeight();
			
		if ( fabs( m_Obstacles[i].center.z - npcLoc.z ) > zDistTooFar )
			continue;

		Vector vecToNpc 		= npcLoc - m_Obstacles[i].center;
		vecToNpc.z = 0;
		float distToObstacleSq 	= sq(vecToNpc.x) + sq(vecToNpc.y);
		float radius = m_Obstacles[i].radius + radiusNpc;

		if ( distToObstacleSq > 0.001 && distToObstacleSq < sq( radius + probeDist ) )
		{
			Vector vecToObstacle = vecToNpc * -1;
			float distToObstacle = VectorNormalize( vecToObstacle );
			float weight;
			float arc;
			float radiusSq = sq(radius);

			float flDot = DotProduct( vecToObstacle, moveGoal.dir );

			// Don't steer around to avoid obstacles we've already passed, unless we're right up against them.
			// That is, do this computation without the probeDist added in.
			if( flDot < 0.0f && distToObstacleSq > radiusSq )
			{
				continue;
			}

			if ( radiusSq < distToObstacleSq )
			{
				Vector vecTangent;
				float distToTangent = FastSqrt( distToObstacleSq - radiusSq );

				float oneOverDistToObstacleSq = 1 / distToObstacleSq;

				vecTangent.x = ( -distToTangent * vecToNpc.x + radius * vecToNpc.y ) * oneOverDistToObstacleSq;
				vecTangent.y = ( -distToTangent * vecToNpc.y - radius * vecToNpc.x ) * oneOverDistToObstacleSq;
				vecTangent.z = 0;

				float cosHalfArc = vecToObstacle.Dot( vecTangent );
				arc = RAD2DEG(acosf( cosHalfArc )) * 2.0;
				weight = 1.0 - (distToObstacle - radius) / probeDist;
				if ( weight > 0.75 )
					arc += (arc * 0.5) * (weight - 0.75) / 0.25;
				
				Assert( weight >= 0.0 && weight <= 1.0 );

#if DEBUG_OBSTACLES
				// -------------------------
				Msg( "Adding arc %f, w %f\n", arc, weight );

				Vector pointTangent = npcLoc + ( vecTangent * distToTangent );
					
				NDebugOverlay::Line( npcLoc - Vector( 0, 0, 64 ), npcLoc + Vector(0,0,64), 0,255,0, false, 0.1 );
				NDebugOverlay::Line( center - Vector( 0, 0, 64 ), center + Vector(0,0,64), 0,255,0, false, 0.1 );
				NDebugOverlay::Line( pointTangent - Vector( 0, 0, 64 ), pointTangent + Vector(0,0,64), 0,255,0, false, 0.1 );
				
				NDebugOverlay::Line( npcLoc + Vector(0,0,64), center + Vector(0,0,64), 0,0,255, false, 0.1 );
				NDebugOverlay::Line( center + Vector(0,0,64), pointTangent + Vector(0,0,64), 0,0,255, false, 0.1 );
				NDebugOverlay::Line( pointTangent + Vector(0,0,64), npcLoc + Vector(0,0,64), 0,0,255, false, 0.1 );
#endif
			}
			else
			{
				arc = 210;
				weight = 1.0;
			}

			if ( m_Obstacles[i].hEntity != NULL )
			{
				weight = AdjustRegulationWeight( m_Obstacles[i].hEntity, weight );
			}
//.........这里部分代码省略.........

//TODO: Override it from the base class
bool calculateDirection(const sensor_msgs::LaserScan::ConstPtr& scan, float& finalDirection) {
	float nearestDistance = MAX_DIST;

	//zapamiętaj pozycję w tabeli, do której odnosi się najlepszy pomiar
	int nearestPosition;
	
	float direction;
	
	//liczba pomiarów
	int count = scan->scan_time / scan->time_increment;

	
	//calculate nearest direction
	for(int i = 0; i < count; i++) {
		float distance = scan->ranges[i];
		if ((distance < nearestDistance) && (distance > MIN_DIST)) {
			nearestDistance = distance;
			direction = scan->angle_min + scan->angle_increment * i;
			nearestPosition = i;
		}
	}
	
	int pi_8 = count / 8;  // 1/8 pełnego kąta - 45 stopni
	
	//calculating direction for avoiding an obstacle
	if (nearestDistance < MAX_DIST) {
		float distanceLeft;
		float distanceRight;
		
		//jeżeli żadna wartość nie przekroczy SAFE MOVE to wybierz best Position
		float bestDirection;
		
		// if any direction is better it will move in that direction
		float bestDistance = nearestDistance;
		
		//all directions except +/- 180 degrees 
		for (int i = 1; i < 4; i++) { 
			int positionLeft = nearestPosition - i * pi_8;
			if(positionLeft < 0) 
				positionLeft = nearestPosition + (8-i)*pi_8;
			
			distanceLeft = scan->ranges[positionLeft];
			
			int positionRight = nearestPosition + i * pi_8;
			if(positionRight >= count)
				positionRight = nearestPosition - (8-i)*pi_8;
				
			distanceRight = scan->ranges[positionRight];
			
			//this is the best choice and return from loop
			// if distance in this direction is greater than safe move
			if (distanceLeft >= nearestDistance + SAFE_MOVE || distanceRight >= nearestDistance + SAFE_MOVE) {
				if (distanceLeft > distanceRight) {
					finalDirection = scan->angle_min + scan->angle_increment * positionLeft;
				}
				else {
					finalDirection = scan->angle_min + scan->angle_increment * positionRight;
				}
				ROS_INFO("move in direction %f, nearest point %f", RAD2DEG(finalDirection), nearestDistance);
				return true;
			}
			else {
				if (distanceLeft > bestDistance) {
					bestDistance = distanceLeft;
					bestDirection =  scan->angle_min + scan->angle_increment * positionLeft;
				}
				if (distanceRight > bestDistance) {
					bestDistance = distanceRight;
					bestDirection = scan->angle_min + scan->angle_increment * positionRight;
				}
			} 
		}
		
		float distance;
		float position;
		position = nearestPosition - 4 * pi_8;
		if(position < 0)
			position = nearestPosition + 4 * pi_8;
			
		distance = scan->ranges[position];
		if (distance > bestDistance) {
			bestDistance = distance;
			bestDirection = scan->angle_min + scan->angle_increment * position;
		}
		
		finalDirection = bestDirection;
		
		ROS_INFO("move in direction %f, nearest point %f", RAD2DEG(finalDirection), nearestDistance);
		return true;
	}
	else {
		ROS_INFO("stable");
		return false;
	}
}

static void prvComputeTask(void* pvParameters)
{
  xCANMsg msg;
  accelero_values_t accelero_values;
  double gyroscope_y = 0;
  double accelero_x = 0;
  double accelero_z = 0;
  portTickType time = xTaskGetTickCount();
  int i = 0;
  static char buf[80];
  static double previous_angle = 0;
  double d = 0;
  int iBtMsgCountdown = 10;

  uart_puts("\r\nSensorboard started...\r\n");
  uart_puts("Processor UID: ");
  uart_puts(itoa(UID, buf));
  uart_puts("\r\n");
  uart_puts("Version: ");
  uart_puts(version);
  uart_puts("\r\n");

  for (;;)
    {
      if (i++ == 100)
        {
          vLEDToggle(LED_YELLOW);
          i = 0;
        }

      if (stopped)
        vTaskDelay(portMAX_DELAY);

      vTaskDelayUntil(&time, 5 / portTICK_RATE_MS);

      accelero_get_values(&accelero_values);

      gyroscope_y = (double)gyroscope_get_value(Y) * PI / 180 / GYROSCOPE_SENSITIVITY;
      accelero_x  = (double)accelero_values.x  / ACCELERO_SENSITIVITY_TYP_6G;
      accelero_z  = (double)accelero_values.z  / ACCELERO_SENSITIVITY_TYP_6G;

      DBGLOG(bDebugKalman, itoa(atan2(accelero_x, -accelero_z) * 1000, buf));
      DBGLOG(bDebugKalman, " ");
      DBGLOG(bDebugKalman, itoa(gyroscope_y * 1000, buf));
      DBGLOG(bDebugKalman, " ");

      xSemaphoreTake(xVMutex, portMAX_DELAY);
      kalman_state_update(gyroscope_y, 0.005);
      xSemaphoreGive(xVMutex);

      kalman_cov_update(accelero_x, accelero_z);

      DBGLOG(bDebugKalman, itoa(kalman_get_angle() * 1000, buf));
      DBGLOG(bDebugKalman, " ");
      DBGLOG(bDebugKalman, itoa(kalman_get_rate() * 1000, buf));
      DBGLOG(bDebugKalman, " ");
      d = dAlpha * d + (1 - dAlpha) * kalman_get_rate();
      DBGLOG(bDebugKalman, itoa(d * 1000, buf));
      DBGLOG(bDebugKalman, "\r\n");
      previous_angle = kalman_get_angle();

      msg.eID = MSG_ANGLE_ANGVEL;
      msg.xData.values.first = kalman_get_angle();
      msg.xData.values.second = d;

      vCANSendMsg(&msg);

      if (bBtInited && !(iBtMsgCountdown--)) {
        int xParameters[1] = {-RAD2DEG(kalman_get_angle())};

        prvBtSendMsg('a', xParameters, 1);

        iBtMsgCountdown = 10;
      }
    }
}

//.........这里部分代码省略.........

		/* This is a new point */
		GetKey(other,other_idx).m_extra = pt_count;
		GetKey(image_idx,this_idx).m_extra = pt_count;

		/* Set up the 3D point */
		v2_t p = v2_new(GetKey(other,other_idx).m_x,
				GetKey(other,other_idx).m_y);
		    
		v2_t q = v2_new(GetKey(image_idx,this_idx).m_x,
				GetKey(image_idx,this_idx).m_y);

                if (m_optimize_for_fisheye) {
                    double p_x = Vx(p), p_y = Vy(p);
                    double q_x = Vx(q), q_y = Vy(q);
                    
                    m_image_data[other].
                        UndistortPoint(p_x, p_y, Vx(p), Vy(p));
                    m_image_data[image_idx].
                        UndistortPoint(q_x, q_y, Vx(q), Vy(q));
                }

		double proj_error = 0.0;
		bool in_front = false;
		double angle = 0.0;

		points[pt_count] = 
		    Triangulate(p, q, cameras[i], cameras[camera_idx], 
				proj_error, in_front, angle, true);


		/* Check that the angle between the rays is large
		 * enough */
		if (RAD2DEG(angle) < m_ray_angle_threshold) {
		    printf(" Ray angle %d => %d is too small (%0.3f)\n", 
			   this_idx, other_idx, RAD2DEG(angle));

		    /* Remove point */
		    GetKey(other,other_idx).m_extra = -1;
		    GetKey(image_idx,this_idx).m_extra = -1;

		    continue;
		}

		/* Check the reprojection error */
		if (proj_error >= ADD_REPROJECTION_ERROR) {
		    printf("  Projection error for %d => %d is %0.3e, "
			   "removing\n",
			   this_idx, other_idx, proj_error);

		    /* Remove point */
		    GetKey(other,other_idx).m_extra = -2;
		    GetKey(image_idx,this_idx).m_extra = -2;

		    continue;
		}

		/* Check cheirality */
		if (!in_front) {
		    printf("  Cheirality violated!\n");

		    /* Remove point */
		    GetKey(other,other_idx).m_extra = -2;
		    GetKey(image_idx,this_idx).m_extra = -2;

		    continue;

//.........这里部分代码省略.........
		int camera_idx1 = new_tracks[i][j].first;
		int image_idx1 = added_order[camera_idx1];
		int key_idx1 = new_tracks[i][j].second;

		int camera_idx2 = new_tracks[i][k].first;
		int image_idx2 = added_order[camera_idx2];
		int key_idx2 = new_tracks[i][k].second;

		Keypoint &key1 = GetKey(image_idx1, key_idx1);
		Keypoint &key2 = GetKey(image_idx2, key_idx2);

		v2_t p = v2_new(key1.m_x, key1.m_y);
		v2_t q = v2_new(key2.m_x, key2.m_y);

                if (m_optimize_for_fisheye) {
                    double p_x = Vx(p), p_y = Vy(p);
                    double q_x = Vx(q), q_y = Vy(q);
                    
                    m_image_data[image_idx1].
                        UndistortPoint(p_x, p_y, Vx(p), Vy(p));
                    m_image_data[image_idx2].
                        UndistortPoint(q_x, q_y, Vx(q), Vy(q));
                }

		double angle = ComputeRayAngle(p, q, 
					       cameras[camera_idx1], 
					       cameras[camera_idx2]);

		if (angle > max_angle)
		    max_angle = angle;

		/* Check that the angle between the rays is large
		 * enough */
		if (RAD2DEG(angle) >= m_ray_angle_threshold) {
		    conditioned = true;
		}

#if 0
		double dist_jk = 
		    GetCameraDistance(cameras + j, cameras + k, 
				      m_explicit_camera_centers);

		if (dist_jk > m_min_camera_distance_ratio * reference_baseline)
		    good_distance = true;
#else
                good_distance = true;
#endif
	    }
	}
	
	if (!conditioned || !good_distance) {
	    num_ill_conditioned++;

#if 0
	    printf(">> Track is ill-conditioned [max_angle = %0.3f]\n", 
		   RAD2DEG(max_angle));
	    fflush(stdout);
#endif
	    continue;
	}
	
	double error;
	v3_t pt;

        if (!m_panorama_mode) {
            pt = TriangulateNViews(new_tracks[i], added_order, cameras, 

void CPlayerRotation::ClampAngles()
{
	{
		//cap up/down looking
		float minAngle,maxAngle;
		GetStanceAngleLimits(minAngle,maxAngle);

		float currentViewPitch=GetLocalPitch();
		float newPitch = currentViewPitch + m_deltaAngles.x;

		if(newPitch < minAngle)
			newPitch = minAngle;
		else if(newPitch > maxAngle)
			newPitch = maxAngle;

		m_deltaAngles.x = newPitch - currentViewPitch;

	}

	{
		//further limit the view if necessary
		float limitV = m_params.vLimitRangeV;
		float limitH = m_params.vLimitRangeH;
		Vec3  limitDir = m_params.vLimitDir;
		float limitVUp = m_params.vLimitRangeVUp;
		float limitVDown = m_params.vLimitRangeVDown;

		if(m_player.m_stats.isFrozen.Value())
		{
			float clampMin = g_pGameCVars->cl_frozenAngleMin;
			float clampMax = g_pGameCVars->cl_frozenAngleMax;
			float frozenLimit = DEG2RAD(clampMin + (clampMax-clampMin)*(1.f-m_player.GetFrozenAmount(true)));

			if(limitV == 0 || limitV>frozenLimit)
				limitV = frozenLimit;

			if(limitH == 0 || limitH>frozenLimit)
				limitH = frozenLimit;

			if(g_pGameCVars->cl_debugFr