• 设为首页
  • 点击收藏
  • 手机版
    手机扫一扫访问
    迪恩网络手机版
  • 关注官方公众号
    微信扫一扫关注
    公众号

C++ RAD2DEG函数代码示例

原作者: [db:作者] 来自: [db:来源] 收藏 邀请

本文整理汇总了C++中RAD2DEG函数的典型用法代码示例。如果您正苦于以下问题:C++ RAD2DEG函数的具体用法?C++ RAD2DEG怎么用?C++ RAD2DEG使用的例子?那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。



在下文中一共展示了RAD2DEG函数的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。

示例1: Euler_direction2angles

	//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*/
开发者ID:dtegunov,项目名称:liblion,代码行数:91,代码来源:euler.cpp


示例2: plan_run_AutoCruise

/**
 * @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);
}
开发者ID:B-robur,项目名称:OpenPilot,代码行数:65,代码来源:plans.c


示例3: m_transitionType


//.........这里部分代码省略.........
			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
							{
开发者ID:Xydrel,项目名称:Infected,代码行数:67,代码来源:MovementTransitions.cpp


示例4: ROS_INFO

    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();
    }

    }
开发者ID:MarcoMura85,项目名称:VisualHatpic,代码行数:83,代码来源:path_planning_widget.cpp


示例5: ThresholdTwists

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);
                }
            }
        }
    }
}
开发者ID:alvisespano,项目名称:stereorecon,代码行数:82,代码来源:TwoFrameModel.cpp


示例6: atof


//.........这里部分代码省略.........
	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;
}
开发者ID:opros-wiki,项目名称:OPRoS_v1_Components,代码行数:101,代码来源:CNR_7DOFAnalyticInverseKinematicsComp.cpp


示例7: SimCarWallCollideResponse


//.........这里部分代码省略.........
		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;
}
开发者ID:andypassion,项目名称:torcs,代码行数:101,代码来源:collide.cpp


示例8: ProcessTestingVars

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);
	}
}
开发者ID:germanocaldeira,项目名称:no-one-lives-forever,代码行数:85,代码来源:CameraOffsetMgr.cpp


示例9: OVR_CalculateState

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;
}
开发者ID:Nephatrine,项目名称:nephq2,代码行数:85,代码来源:r_vr_ovr.c


示例10: GetNpc

//-----------------------------------------------------------------------------
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 );
			}
//.........这里部分代码省略.........
开发者ID:0xFEEDC0DE64,项目名称:UltraGame,代码行数:101,代码来源:ai_planesolver.cpp


示例11: calculateDirection

//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;
	}
}
开发者ID:bitcoinsoftware,项目名称:UrbanDroneSystem,代码行数:96,代码来源:bestOfFour.cpp


示例12: prvComputeTask

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;
      }
    }
}
开发者ID:RoseWheel,项目名称:RoseWheel,代码行数:76,代码来源:main.c


示例13: MIN


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

		/* 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;
开发者ID:wy342786117,项目名称:bundler_sfm,代码行数:67,代码来源:BundleAdd.cpp


示例14: GetNumKeys


//.........这里部分代码省略.........
		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, 
开发者ID:wy342786117,项目名称:bundler_sfm,代码行数:67,代码来源:BundleAdd.cpp


示例15: GetStanceAngleLimits

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 

鲜花

握手

雷人

路过

鸡蛋
该文章已有0人参与评论

请发表评论

全部评论

专题导读
上一篇:
C++ RADEON_CONTEXT函数代码示例发布时间:2022-05-30
下一篇:
C++ RAD函数代码示例发布时间:2022-05-30
热门推荐
阅读排行榜

扫描微信二维码

查看手机版网站

随时了解更新最新资讯

139-2527-9053

在线客服(服务时间 9:00~18:00)

在线QQ客服
地址:深圳市南山区西丽大学城创智工业园
电邮:jeky_zhao#qq.com
移动电话:139-2527-9053

Powered by 互联科技 X3.4© 2001-2213 极客世界.|Sitemap