bool CLaser::GetFiringDir(EntityId weaponId, const IFireMode* pFireMode, Vec3& dir, const Vec3& probableHit, const Vec3& firingPos)
{
	CWeapon* pParentWeapon = GetWeapon();

	if (!m_laserBeam.IsLaserActivated())
		return false;

	if(gEnv->bMultiplayer && pParentWeapon && pParentWeapon->IsZoomed())
	{
		return false;
	}

	if(!probableHit.IsZero() && !firingPos.IsZero())
	{
		dir = (probableHit - firingPos).GetNormalized();

		return true;
	}


	if (pParentWeapon)
	{
		int slot = pParentWeapon->IsOwnerFP() ? eIGS_FirstPerson : eIGS_ThirdPerson;

		Matrix33 rotation = pParentWeapon->GetSlotHelperRotation(slot, "weapon_term", true);

		dir = rotation.GetColumn1();
		return true;
	}

	return false;
}
bool CLaser::GetProbableHit(EntityId weaponId, const IFireMode* pFireMode, Vec3& hit)
{
	if (!m_laserBeam.IsLaserActivated())
		return false;

	if(gEnv->bMultiplayer)
	{
		CWeapon* pParentWeapon = GetWeapon();
		if (pParentWeapon && pParentWeapon->IsZoomed())
		{
			return false;
		}
	}

	CWeapon* pWeapon = GetWeapon();
	int slot = pWeapon->IsOwnerFP() ? eIGS_FirstPerson : eIGS_ThirdPerson;

	Vec3 lastBeamHit = m_laserBeam.GetLastHit();
	Vec3 currentBeamPosition = pWeapon->GetSlotHelperPos(slot, "weapon_term", true);
	Matrix33 rotation = pWeapon->GetSlotHelperRotation(slot, "weapon_term", true);
	Vec3 currentBeamDirection = rotation.GetColumn1();

	const CFireMode* pCFireMode = static_cast<const CFireMode*>(pFireMode);
	const CSingle* pSingle = crygti_cast<const CSingle*>(pCFireMode);
	if (pSingle && pSingle->GetShared()->fireparams.laser_beam_uses_spread)
	{
		currentBeamDirection = pSingle->ApplySpread(currentBeamDirection, pSingle->GetSpread());
	}

	float distanceToLastHit = lastBeamHit.GetDistance(currentBeamPosition);
	hit = currentBeamPosition + currentBeamDirection * distanceToLastHit;

	return true;
}
Пример #3
0
	void getSigma(MATRIX33* dest,MATRIX33* F_in){
		Matrix33 F(F_in);
		for(unsigned i=0;i<3;i++)
			F(i,i)+=1.0;
		double J=F.I3();

		Matrix33 F_T=F.transpose();
		Matrix33 C=F_T*F;
		double I_C  =C.I1();
		double II_C =C.I2();
		double III_C=C.I3();

		Matrix33 C_inv=C.inv();

		Matrix33 I;
		I.identity();

		Matrix33 c1_mat,c2_mat;
		{
			c1_mat=-1./3.*I_C*C_inv;
			for(unsigned i=0;i<3;i++)
				c1_mat(i,i)+=1.;
			c1_mat*=c1*pow(III_C,-1./3.);
		}{
			c2_mat=I_C*I-C-2./3.*II_C*C_inv;
			c2_mat*=c2*pow(III_C,-2./3.);
		}

		Matrix33 S=c1_mat+c2_mat;
		Matrix33 tau=F*S*F_T;

		for(int i=0;i<3;i++)
			for(int j=0;j<3;j++)
				(*dest)(i,j)=tau(i,j)/J*2.;
	};
Пример #4
0
int main(int argc,char **argv)
{
	GLUTWrapper::setApplication(new TestApp);
	GLUTWrapper::run(argc,argv);

	Matrix33 m(3,1,0,-1,2,2,5,0,-1);
	//Matrix33 m(3,3,6,0,1,2,-2,0,0);
	std::cout << "MATRIX:\n" << m << std::endl;
	std::cout << "MATRIX^-1:\n" << m.inverse() << std::endl;
	Matrix33 mxi = m * m.inverse(Matrix33::GAUSS_JORDAN);
	mxi.clean();
	std::cout << "MATRIX x MATRIX^-1:\n" << mxi << std::endl;

	/*try
	{
		PPMImage img("abe_natsumi.ppm");
	}
	catch(Exception e)
	{
		std::cout << e.what() << std::endl;
	}*/

	//inversion_tests();

	//big_matrix_inverse();

	//test();

	while(!_kbhit()) ;

	return 0;
}
Пример #5
0
//------------------------------------------------------------------------
const Matrix33 &CItem::GetSlotHelperRotation(int slot, const char *helper, bool worldSpace, bool relative)
{
	static Matrix33 rotation;
	rotation.SetIdentity();

	IEntity* pEntity = GetEntity();
	if(!pEntity)
		return rotation;

	SEntitySlotInfo info;
	if (pEntity->GetSlotInfo(slot, info))
	{
    if (info.pStatObj)
    {
      IStatObj *pStatObj = info.pStatObj;
      rotation = Matrix33(pStatObj->GetHelperTM(helper));
      rotation.OrthonormalizeFast();
      rotation = Matrix33(GetEntity()->GetSlotLocalTM(slot, false))*rotation;        
    }
		else if (info.pCharacter)
		{
			ICharacterInstance *pCharacter = info.pCharacter;
			if(!pCharacter)
				return rotation;

			IAttachment* pAttachment = pCharacter->GetIAttachmentManager()->GetInterfaceByName(helper);
			if(pAttachment)
			{
				rotation = Matrix33(worldSpace ? pAttachment->GetAttWorldAbsolute().q : pAttachment->GetAttModelRelative().q);
				return rotation;
			}
			else
			{
				ICharacterModelSkeleton* pICharacterModelSkeleton = pCharacter->GetICharacterModel()->GetICharacterModelSkeleton();
				ISkeletonPose* pSkeletonPose = pCharacter->GetISkeletonPose();
				int16 id = pICharacterModelSkeleton->GetJointIDByName(helper);
				if (id > -1)
				{
					rotation = relative ? Matrix33(pSkeletonPose->GetRelJointByID(id).q) : Matrix33(pSkeletonPose->GetAbsJointByID(id).q);
				}
			}

			if (!relative)
			{
				rotation = Matrix33(pEntity->GetSlotLocalTM(slot, false)) * rotation;
			}
		}    
	}

	if (worldSpace)
	{
		rotation = Matrix33(pEntity->GetWorldTM()) * rotation;
	}

	return rotation;
}
Пример #6
0
	void OpenGLContext::scale( VGfloat sx, VGfloat sy ) {
		Matrix33* active = getActiveMatrix();
		Matrix33 scale;
		scale.setIdentity();
		scale.setScale( sx, sy );
        Matrix33 tmp;
        Matrix33::multiply( tmp, scale, *active );
		active->copy( tmp );
		loadGLMatrix();
	}
inline Matrix33 horizontal( const Matrix33& src )
{
	Matrix33 dest = src;
	dest( 1, 1 ) = 1;
	dest( 0, 1 ) = 0;
	dest( 2, 1 ) = 0;
	dest( 1, 0 ) = 0;
	dest( 1, 2 ) = 0;
	dest.Orthonormalize();
	return dest;
}
Пример #8
0
void Exporter::objectTransform(QuaternionXYZW &rot, Vector3 &trans, INode *node, TimeValue t, bool local)
{
   Matrix33 rm;
   objectTransform(rm, trans, node, t, local);

   Quaternion q = rm.AsQuaternion();
   rot.x = q.x;
   rot.y = q.y;
   rot.z = q.z;
   rot.w = q.w;
}
Пример #9
0
	void OpenGLContext::multiply( const VGfloat* t ) {
		Matrix33 m;
		for ( int x = 0; x < 3; x++ ) {
			for ( int y = 0; y < 3; y++ ) {
				m.set( y, x, t[(y*3)+x] );
			}
		}
		Matrix33* active = getActiveMatrix();
		active->postMultiply( m );
		loadGLMatrix();
	}
Пример #10
0
//------------------------------------------------------------------------
const Matrix33 &CItem::GetSlotHelperRotation(int slot, const char *helper, bool worldSpace, bool relative)
{
	// if mounted force the slot to be 1st person
	if(m_stats.mounted)
		slot=eIGS_FirstPerson;

	static Matrix33 rotation;
	rotation.SetIdentity();

	IEntity *pEntity = GetEntity();

	if(!pEntity)
		return rotation;

	SEntitySlotInfo info;

	if(pEntity->GetSlotInfo(slot, info))
	{
		if(info.pStatObj)
		{
			IStatObj *pStatObj = info.pStatObj;
			rotation = Matrix33(pStatObj->GetHelperTM(helper));
			rotation.OrthonormalizeFast();
			rotation = Matrix33(GetEntity()->GetSlotLocalTM(slot, false))*rotation;
		}
		else if(info.pCharacter)
		{
			ICharacterInstance *pCharacter = info.pCharacter;

			if(!pCharacter)
				return rotation;

			int16 id = pCharacter->GetISkeletonPose()->GetJointIDByName(helper);

			//	if (id > -1) rotation = Matrix33(pCharacter->GetISkeleton()->GetAbsJMatrixByID(id));
			if(id > -1)
			{
				if(relative)
					rotation = Matrix33(pCharacter->GetISkeletonPose()->GetRelJointByID(id).q);
				else
					rotation = Matrix33(pCharacter->GetISkeletonPose()->GetAbsJointByID(id).q);
			}

			if(!relative)
				rotation = Matrix33(pEntity->GetSlotLocalTM(slot, false))*rotation;
		}
	}

	if(worldSpace)
		rotation=Matrix33(pEntity->GetWorldTM())*rotation;

	return rotation;
}
Пример #11
0
void CRotatableTurret::Update( float dt )
{
	UpdateAimInfo();

	Matrix34 init_world_pose
		= m_ParentWorldPose * m_MountLocalPose;

	// Why calculate on local space?
	// - world pose of the turret keeps changes if the owner entity keeps moving.

//	Vector3 vTargetDirection = m_vAimDirection;
	Vector3 vLocalTTableAxis = m_MountLocalPose.matOrient.GetColumn(1); // y-axis

	Vector3 vLocalTargetDir;
	init_world_pose.InvTransform( vLocalTargetDir, m_vAimDirection );

	Vector3 vLocalTgtDirOnTTable
		= vLocalTargetDir - vLocalTTableAxis * Vec3Dot( vLocalTTableAxis, vLocalTargetDir );

	Vec3Normalize( vLocalTgtDirOnTTable, vLocalTgtDirOnTTable );

	Matrix33 matLocalTargetRotation;
	matLocalTargetRotation.SetColumn( 0, Vec3Cross( vLocalTTableAxis, vLocalTgtDirOnTTable ) );
	matLocalTargetRotation.SetColumn( 1, vLocalTTableAxis );
	matLocalTargetRotation.SetColumn( 2, vLocalTgtDirOnTTable );
	matLocalTargetRotation.Orthonormalize();

	m_LocalTurnTableOrient.target.FromRotationMatrix( matLocalTargetRotation );

	// calc how much the gun tube needs to be elevated (pitch rotation)

	float target_pitch = Vec3GetAngleBetween( vLocalTargetDir, vLocalTgtDirOnTTable );

	Limit( target_pitch, 0.0f, deg_to_rad(85.0f) );

	m_LocalGunTubePitchAngle.target = target_pitch;

	// update critical damping
	m_LocalGunTubePitchAngle.Update( dt );
	m_LocalTurnTableOrient.Update( dt );

//	vLocalTTableRightDir = m_LocalTurnTableOrient.current.ToRotationMatrix().GetColumn(0);

	// update local poses
	Matrix34 local_mount_pose, local_gun_tube_pose;
	local_mount_pose    = Matrix34( m_MountLocalPose.vPosition, m_LocalTurnTableOrient.current.ToRotationMatrix() );
//	local_gun_tube_pose = Matrix34( m_GunLocalPose.vPosition,   Matrix33RotationAxis( m_LocalGunTubePitchAngle.current, vLocalTTableRightDir ) );
	local_gun_tube_pose = Matrix34( m_GunLocalPose.vPosition,   Matrix33RotationAxis( m_LocalGunTubePitchAngle.current, m_GunLocalPose.matOrient.GetColumn(0) ) );

	m_MountWorldPose = m_ParentWorldPose * local_mount_pose;
	m_GunWorldPose   = m_MountWorldPose * local_gun_tube_pose;
}
Пример #12
0
Dvoid Camera::ViewMatrixCalculate()
{
    Matrix33 rotate;
    rotate.SetColumns(m_vDir[CD_RIGHT], m_vDir[CD_UP], -m_vDir[CD_FRONT]);

    rotate.Transpose();
    m_matCam[CM_VIEW].Rotation(rotate);

    // set translation (rotate into view space)
    Vector3 invEye = -(rotate* m_vPos);
    m_matCam[CM_VIEW](0, 3) = invEye.x;
    m_matCam[CM_VIEW](1, 3) = invEye.y;
    m_matCam[CM_VIEW](2, 3) = invEye.z;
}
Пример #13
0
bool CollisionBoxComponent::CheckCollision(const Vector2<float>& aPosition) const
{
	Matrix33<float> localSpace = myParent->GetSpace();


	if (myLineVolume.Inside(aPosition - (myPosition + localSpace.GetPosition())) == true)
	{
		return true;
	}
	else
	{
		return false;
	}
}
Пример #14
0
	void CoreLayer2D_Imp::DrawAdditionalObjects()
	{

#if __CULLING_2D__
		//一時的にエフェクトは無条件に描画(本来ここではない)
		for (auto& x : m_objects)
		{
			if (x->GetIsAlive() && x->GetObjectType() == Object2DType::Effect)
			{
				x->Draw(m_renderer);
			}
		}
#endif

		for (auto& sprite : sprites)
		{
			m_renderer->AddSprite(
				sprite.pos.data(),
				sprite.col.data(),
				sprite.uv.data(),
				sprite.Texture_.get(),
				sprite.AlphaBlend_,
				sprite.Priority);
		}

		for (auto& text : texts)
		{
			Matrix33 matP;
			Matrix33 mat;
			mat.SetTranslation(text.Position_.X, text.Position_.Y);

			m_renderer->AddText(
				matP,
				mat,
				Vector2DF(),
				false,
				false,
				text.Color_,
				text.Font_.get(),
				text.Text_.c_str(),
				text.WritingDirection_,
				text.AlphaBlend_,
				text.Priority_,
				0,
				0);
		}
	}
void InitIncrementalPose::initPose(const Matrix33& R, const Vector31& T, Pose& pose) const {
	pose.sett(RealPoint3D<double>(T.GetX(), T.GetY(), T.GetZ()));

	Matrix<double>& pR = pose.R();
	pR[0][0] = R.Get00(); pR[1][0] = R.Get01(); pR[2][0] = R.Get02();
	pR[0][1] = R.Get10(); pR[1][1] = R.Get11(); pR[2][1] = R.Get12();
	pR[0][2] = R.Get20(); pR[1][2] = R.Get21(); pR[2][2] = R.Get22();

	pose.calcEulerAngles();

	pose.setorientationSynchronWithAngles(true);
	pose.setderivationsSynchronWithAngles(false);
}
Пример #16
0
        void readOOBB(std::string filePath, Vector3& minP, Vector3& maxP, Matrix33& R_KI, Vector3List& pointList)
        {
            auto v = getPointsFromFile3D(filePath);
            if(v.size() != 5 + 8)
            {
                ApproxMVBB_ERRORMSG("Wrong number of points in OOBB file: " << filePath << " points: " << v.size())
            }
            minP        = v[0];
            maxP        = v[1];
            R_KI.row(0) = v[2];
            R_KI.row(1) = v[3];
            R_KI.row(2) = v[4];

            pointList.resize(8);
            for(unsigned int i = 0; i < 8; ++i)
            {
                pointList[i] = v[5 + i];
            }
        }
Пример #17
0
void Exporter::convertMatrix(Matrix33 &dst, const Matrix3 &src)
{
	Point3 r0 = src.GetRow(0);
	Point3 r1 = src.GetRow(1);
	Point3 r2 = src.GetRow(2);

	dst.Set(r0.x, r0.y, r0.z,
		    r1.x, r1.y, r1.z,
			r2.x, r2.y, r2.z);
}
bool CustomizedJointPath::calcInverseKinematics(const Vector3& end_p, const Matrix33& end_R)
{
    bool solved;
	
    if(ikTypeId == 0 || isBestEffortIKMode){

        solved = JointPath::calcInverseKinematics(end_p, end_R);

    } else {

        std::vector<double> qorg(numJoints());
		
        for(int i=0; i < numJoints(); ++i){
            qorg[i] = joint(i)->q;
        }

        const Link* targetLink = endLink();
        const Link* baseLink_ = baseLink();

        Vector3 p_relative;
        Matrix33 R_relative;
        if(!isCustomizedIkPathReversed){
            p_relative.noalias() = baseLink_->R.transpose() * (end_p - baseLink_->p);
            R_relative.noalias() = baseLink_->R.transpose() * end_R;
        } else {
            p_relative.noalias() = end_R.transpose() * (baseLink_->p - end_p);
            R_relative.noalias() = end_R.transpose() * baseLink_->R;
        }
        solved = body->customizerInterface->
            calcAnalyticIk(body->customizerHandle, ikTypeId, p_relative, R_relative);

        if(solved){

            calcForwardKinematics();

            Vector3 dp(end_p - targetLink->p);
            Vector3 omega(omegaFromRot(targetLink->R.transpose() * end_R));
			
            double errsqr = dp.dot(dp) + omega.dot(omega);
			
            if(errsqr < maxIKErrorSqr){
                solved = true;
            } else {
                solved = false;
                for(int i=0; i < numJoints(); ++i){
                    joint(i)->q = qorg[i];
                }
                calcForwardKinematics();
            }
        }
    }

    return solved;
}
Пример #19
0
//====================================================================
// DebugDrawWireFOVCone
//====================================================================
void CAIDebugRenderer::DrawWireFOVCone(const Vec3& vPos, const Vec3& vDir, float fRadius, float fFOV, const ColorB& color)
{
	const unsigned int npts = 32;
	const unsigned int npts2 = 16;
	Vec3	points[npts];
	Vec3	pointsx[npts2];
	Vec3	pointsy[npts2];

	Matrix33	base;
	base.SetRotationVDir(vDir);

	float coneRadius = sinf(fFOV) * fRadius;
	float coneHeight = cosf(fFOV) * fRadius;

	for (unsigned int i = 0; i < npts; i++)
	{
		float	a = ((float)i / (float)npts) * gf_PI2;
		float rx = cosf(a) * coneRadius;
		float ry = sinf(a) * coneRadius;
		points[i] = vPos + base.TransformVector(Vec3(rx, coneHeight, ry));
	}

	for (unsigned int i = 0; i < npts2; i++)
	{
		float	a = -fFOV + ((float)i / (float)(npts2-1)) * (fFOV*2);
		float rx = sinf(a) * fRadius;
		float ry = cosf(a) * fRadius;
		pointsx[i] = vPos + base.TransformVector(Vec3(rx, ry, 0));
		pointsy[i] = vPos + base.TransformVector(Vec3(0, ry, rx));
	}

	DrawPolyline(points, npts, true, color);
	DrawPolyline(pointsx, npts2, false, color);
	DrawPolyline(pointsy, npts2, false, color);

	DrawLine(points[0], color, vPos, color);
	DrawLine(points[npts/4], color, vPos, color);
	DrawLine(points[npts/2], color, vPos, color);
	DrawLine(points[npts/2+npts/4], color, vPos, color);
}
Пример #20
0
	//world -> view
	Matrix44 TransformHelper::CreateView(const Vector3& eye, const Vector3& lookAt, const Vector3& up)
	{				
		// compute view vectors
		Vector3 view = lookAt - eye;
		Vector3 right;
		Vector3 viewUp;
		view.Normalize();
		right = view.Cross(up);
		right.Normalize();
		viewUp = right.Cross(view);
		viewUp.Normalize();

		// now set up matrices
		// base rotation matrix
		Matrix33 rotate;
		rotate.SetColumns(right, viewUp, -view);

		// view->world transform
		// set rotation
		//Matrix44 mViewToWorldMatrix;
		//mViewToWorldMatrix.Rotation(rotate);
		//// set translation (eye position)
		//mViewToWorldMatrix(0, 3) = eye.x;
		//mViewToWorldMatrix(1, 3) = eye.y;
		//mViewToWorldMatrix(2, 3) = eye.z;

		// world->view transform
		// set rotation
		Matrix44 mWorldToViewMatrix;
		rotate.Transpose();
		mWorldToViewMatrix.Rotation(rotate);
		// set translation (rotate into view space)
		Vector3 invEye = -(rotate*eye);
		mWorldToViewMatrix(0, 3) = invEye.x;
		mWorldToViewMatrix(1, 3) = invEye.y;
		mWorldToViewMatrix(2, 3) = invEye.z;
					
		return mWorldToViewMatrix;
	}
Пример #21
0
// Helpful print functions
void print_matrix(Matrix33 const &_matrix)
{
  DebugLogPrint("MATRIX:\n");
  for (int i = 0; i < 3; ++i)
  {
    DebugLogPrint("ROW%d:", i);
    for (int j = 0; j < 3; ++j)
    {
      DebugLogPrint(" %f ", _matrix.values[i][j]);
    }
    DebugLogPrint("\n");
  }
  DebugLogPrint("DETERMINANT: %f\n", _matrix.Determinant());
}
Пример #22
0
	void OpenGLContext::rotate( VGfloat angle ) {
		Matrix33* active = getActiveMatrix();
		Matrix33 rotate;
		rotate.setRotation( radians( angle ) );
		Matrix33 tmp;
		tmp.setIdentity();
		Matrix33::multiply( tmp, rotate, *active );
		active->copy( tmp );
		loadGLMatrix();
	}
Пример #23
0
	void OpenGLContext::translate( VGfloat x, VGfloat y ) {
		
		Matrix33* active = getActiveMatrix();
		Matrix33 translate;
		translate.setTranslate( x, y );
		Matrix33 tmp;
		tmp.setIdentity();
		Matrix33::multiply( tmp, translate, *active );
		active->copy( tmp );
		loadGLMatrix();
	}
void CVehicleMovementHelicopter::ProcessAI(const float deltaTime)
{
	FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );

	CryAutoCriticalSection lk(m_lock);
	SVehiclePhysicsStatus* physStatus = &m_physStatus[k_physicsThread];
		
	if (m_arcade.m_handling.maxSpeedForward>0.f) // Use the new handling code
	{
		//ResetActions();
		m_movementAction.Clear();
		m_movementAction.isAI = true;

		SVehiclePhysicsHelicopterProcessAIParams params;
		params.pPhysStatus = physStatus;
		params.pInputAction = &m_inputAction;
		params.pAiRequest = &m_aiRequest;
		params.dt = deltaTime;
		params.aiRequiredVel = m_CurrentVel;
		params.aiCurrentSpeed = m_CurrentSpeed;
		params.aiYawResponseScalar = m_yawResponseScalar;
		m_yawResponseScalar = 1.f;
	
		// Use helper class to process the AI input
		// It will return a requested velocity, and change the input
		m_arcade.ProcessAI(params);

		// Get the output velocity
		m_CurrentVel = params.aiRequiredVel;
		m_CurrentSpeed = params.aiCurrentSpeed;
		return;
	}
	////////////////////// OLD DEPRECATED CODE :( //////////////////////////////////

	m_movementAction.Clear();
	ResetActions();

	// Our current state
	const Vec3 worldPos =  physStatus->pos;
	const Matrix33 worldMat( physStatus->q);
	const Matrix33 localMat( physStatus->q.GetInverted());
	const Ang3 worldAngles = Ang3::GetAnglesXYZ(worldMat);
	const Ang3 localAngles = Ang3::GetAnglesXYZ(localMat);

	const Vec3 currentVel = physStatus->v;
	const Vec3 currentVel2D(currentVel.x, currentVel.y, 0.0f);
	m_CurrentSpeed = m_CurrentVel.len(); //currentVel.len();
  float currentSpeed2d = currentVel2D.len();

	// +ve direction mean rotation anti-clocwise about the z axis - 0 means along y
	float currentDir = worldAngles.z;

	// to avoid singularity
	const Vec3 vWorldDir = worldMat.GetRow(1);
	const Vec3 vSideWays = worldMat.GetRow(0);
	const Vec3 vWorldDir2D =  Vec3( vWorldDir.x,  vWorldDir.y, 0.0f ).GetNormalizedSafe();

	// Our inputs
	float desiredSpeed = m_aiRequest.HasDesiredSpeed() ? m_aiRequest.GetDesiredSpeed() : 0.0f;
	Limit(desiredSpeed, -m_maxSpeed, m_maxSpeed);

	const Vec3 desiredMoveDir = m_aiRequest.HasMoveTarget() ? (m_aiRequest.GetMoveTarget() - worldPos).GetNormalizedSafe() : vWorldDir;
	Vec3 desiredMoveDir2D = Vec3(desiredMoveDir.x, desiredMoveDir.y, 0.0f);
	desiredMoveDir2D = desiredMoveDir2D.GetNormalizedSafe(desiredMoveDir2D);

	const Vec3 desiredVel = desiredMoveDir * desiredSpeed; 
	const Vec3 desiredVel2D(desiredVel.x, desiredVel.y, 0.0f);

  Vec3 desiredLookDir(desiredMoveDir);

  if (m_aiRequest.HasDesiredBodyDirectionAtTarget())
  {
	    desiredLookDir = m_aiRequest.GetDesiredBodyDirectionAtTarget().GetNormalizedSafe(desiredMoveDir);
  }
  else if (m_aiRequest.HasLookTarget())
  {
    desiredLookDir = (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe(desiredMoveDir);  
  }


	//const Vec3 desiredLookDir = m_aiRequest.HasLookTarget() ? (m_aiRequest.GetLookTarget() - worldPos).GetNormalizedSafe() : desiredMoveDir;
	const Vec3 desiredLookDir2D = Vec3(desiredLookDir.x, desiredLookDir.y, 0.0f).GetNormalizedSafe(vWorldDir2D);

  Vec3 prediction = m_aiRequest.HasBodyTarget() ? m_aiRequest.GetBodyTarget() : ZERO;

  prediction = (prediction.IsEquivalent(ZERO)) ? desiredMoveDir2D : prediction - worldPos;
  prediction.z = 0.0f;

  float speedLimit = prediction.GetLength2D();

	if(speedLimit > 0.0f)
	{
		prediction *= 1.0f / speedLimit;
	}

  Vec3 tempDir = currentVel2D.IsEquivalent(ZERO) ? localMat.GetRow(1) : currentVel2D;
  tempDir.z = 0.0f;
  tempDir.NormalizeFast();

	float dotProd = tempDir.dot(prediction);
	Limit(dotProd, FLT_EPSILON, 1.0f);

	float accel = m_enginePowerMax * min(2.0f,  1.0f / dotProd); // * dotProd;

	if (!m_aiRequest.HasDesiredBodyDirectionAtTarget())
	{
		dotProd *= dotProd;
		dotProd *= dotProd;
		float tempf = min(max(speedLimit * speedLimit, 2.0f), m_maxSpeed * dotProd);
		Limit(desiredSpeed, -tempf, tempf);
	}
	else if (dotProd < 0.0125f)
	{
		Limit(desiredSpeed, -m_maxSpeed * 0.25f, m_maxSpeed * 0.25f);
	}
  
  float posNeg = (float)__fsel(desiredSpeed - m_CurrentSpeed, 1.0f, -5.0f);

  if (desiredVel2D.GetLengthSquared() > FLT_EPSILON)
  {
	  m_CurrentSpeed = m_CurrentSpeed + posNeg * accel * deltaTime;
  }
  else
  {
    m_CurrentSpeed = m_CurrentSpeed + posNeg * accel * deltaTime;
  }

	if (posNeg > 0.0f && m_CurrentSpeed > desiredSpeed)
	{
		m_CurrentSpeed = desiredSpeed;
	}
	else if (posNeg < 0.0f && m_CurrentSpeed < desiredSpeed)
	{
		m_CurrentSpeed = desiredSpeed;
	}

	
	// ---------------------------- Rotation ----------------------------
	
	float desiredDir = (desiredLookDir2D.GetLengthSquared() > 0.0f) ? atan2f(-desiredLookDir2D.x, desiredLookDir2D.y) : atan2f(-vWorldDir2D.x, vWorldDir2D.y);
	while (currentDir < desiredDir - gf_PI)
		currentDir += 2.0f * gf_PI;
	while (currentDir > desiredDir + gf_PI)
		currentDir -= 2.0f * gf_PI;

	// ---------------------------- Yaw ----------------------------

	Ang3 dirDiff(0.0f, 0.0f, desiredDir - currentDir);
	dirDiff.RangePI();
	float absDiff   = fabsf(dirDiff.z);

	float rotSpeed		= (float)__fsel(dirDiff.z, m_yawPerRoll, -m_yawPerRoll);
	m_actionYaw		= m_actionYaw + deltaTime * (rotSpeed - m_actionYaw);

	float temp = fabsf(m_actionYaw);

	float multiplier = ((absDiff / (temp + 0.001f)) + 1.0f) * 0.5f;
	
	m_actionYaw	*= (float)__fsel(absDiff - temp, 1.0f, multiplier);

	// ---------------------------- Yaw ------------------------------

	m_CurrentVel = desiredMoveDir * m_CurrentSpeed;

  // ---------------------------- Pitch ----------------------------

  if (m_CurrentVel.GetLengthSquared2D() > 0.1f)
  {
    CalculatePitch(worldAngles, desiredMoveDir, currentSpeed2d, desiredSpeed, deltaTime);
  }
  else
  {
    Quat rot;
    rot.SetRotationVDir(desiredLookDir, 0.0f);
    float desiredXRot = Ang3::GetAnglesXYZ(rot).x + m_steeringDamage.x;

    m_actionPitch = worldAngles.x + (desiredXRot - worldAngles.x) * deltaTime/* * 10.0f*/;
    Limit(m_actionPitch, -m_maxPitchAngle * 2.0f, m_maxPitchAngle * 2.0f);
  }


	// ---------------------------- Roll ----------------------------
	float rollSpeed = GetRollSpeed();

	rollSpeed *= deltaTime;
	rollSpeed		= (float)__fsel(absDiff - rollSpeed, rollSpeed, absDiff);
	float roll		=(float) __fsel(dirDiff.z, -rollSpeed, rollSpeed);

	float speedPerUnit	   = 1.5f;
	float desiredRollSpeed = absDiff * speedPerUnit * (float)__fsel(dirDiff.z, 1.0f, -1.0f);
  
	desiredRollSpeed = -m_actionYaw * 2.5f;
  desiredRollSpeed += m_steeringDamage.y;

	m_actionRoll = m_actionRoll + deltaTime * (desiredRollSpeed - m_actionRoll);
	Limit(m_actionRoll, -m_maxRollAngle + m_steeringDamage.y, m_maxRollAngle - m_steeringDamage.y);
	m_actionRoll *= m_rollDamping;
	// ---------------------------- Roll ----------------------------


	// ---------------------------- Convert and apply ----------------------------
	Ang3 angles(m_actionPitch, m_actionRoll, worldAngles.z + deltaTime * m_actionYaw);
	
	pe_params_pos paramPos;
	paramPos.q.SetRotationXYZ(angles);
	paramPos.q.Normalize();

	IPhysicalEntity * pPhysicalEntity = GetPhysics();
	pPhysicalEntity->SetParams(&paramPos, 1);

	pe_action_set_velocity vel;
	vel.v = m_CurrentVel + m_netPosAdjust;
	pPhysicalEntity->Action(&vel, 1);

	// ---------------------------- Convert and apply ----------------------------

  m_rpmScale = max(0.2f, cry_fabsf(m_CurrentSpeed / m_maxSpeed));
}
Пример #25
0
void HitSpinnerWall( Spinner* sp, Plane &plane, Ball* b )
{
	
	Vector3 wall = plane.points[2] - plane.points[0];
	Vector3 ballFromWall = b->Position() - plane.points[0];

	float relativePosition = plane.normal * ballFromWall;
	float relativeDirection = plane.normal * b->Velocity();
	if ((relativePosition > 0 && relativeDirection < 0) || (relativePosition < 0 && relativeDirection > 0))
	{
		float theta, c, s;
		theta = acos((wall * Vector3(1,0,0))/wall.getLength());
		if (b->position.x<0)
		{
			theta *= -1;
		}
		c = cos(theta), s = sin(theta);

		Vector3 position2 = Vector3();
		Vector3 velocityRot = Vector3();

		position2 = Vector3(c * ballFromWall.x + s * ballFromWall.y, c * ballFromWall.y - s * ballFromWall.x, 0);
		velocityRot = Vector3( c * b->Velocity().x + s * b->Velocity().y, c * b->Velocity().y - s * b->Velocity().x, 0);

		// Set the ball outside the wall, reverse velocity toward wall
		Vector3 scaledNormal = b->radius * Vector3(0,-1,0);
		position2.y = scaledNormal.y;
		velocityRot.y *= -1;

		// Rotate back
		ballFromWall = Vector3(c * position2.x - s * position2.y,c * position2.y + s * position2.x,0);
		b->Velocity(Vector3(c * velocityRot.x - s * velocityRot.y,c * velocityRot.y + s * velocityRot.x,0));

		Point3 newPos = plane.points[0] + ballFromWall;
		b->position = newPos;

		//float bEnergy = .5 * b->Mass() * b->Velocity() * b->Velocity();
		Vector3 momenteum = b->Mass() * b->Velocity();

		Point3 closest = plane.ClosestPointToPoint(b->Position());
		//Vector3 tempV = diff;
		//tempV.normalize();
		//Vector3 proj = (closest*tempV)*tempV;
		//float temp = proj.getLength();
		//// check if in the bounding box
		//if(diff.getLength() > temp + b->radius)
		//{
		//	continue;
		//}
		//// then it needs to go, "Hey!".


		////find the force applied to the bar
		// ft = dp/dt = m*dv/dt
		////find perpendicular momentum
		//Vector3 totalForce = bEnergy * b->Velocity();

		//// t = r X F
		//Vector3 torque = diff.cross(totalForce);
		Vector3 radius = Point3(closest.x,closest.y,closest.z) - sp->Position();
		Vector3 torque = radius.cross(momenteum);
		
		Vector3 dL = .05 * torque;

		//// find the rotation matrix and the inverse of the rotation matrix
		Matrix33 myRotation = Matrix33(sp->Rotation());
		Matrix33 myRotationT = myRotation.Transpose();

		sp->MyL(sp->MyL() + dL);

		////w(i+1) = q(i+1) * I(-1) * qT(i+1) * L(i+1)
		sp->AngularVel(myRotation * *sp->Tensor()->inverse() * myRotationT * sp->MyL());

		////q(n+1) = q(n) + h(.5 * w(n)*q(n))
		sp->Rotation(sp->Rotation() + .05f*(.5 * Quaternion(sp->AngularVel()) * sp->Rotation()));
		Quaternion q = sp->Rotation();
		q.normalize();
		sp->Rotation(q);
	}
}
Пример #26
0
void GhostBody::SetGeomRotation(Matrix33 const & matrix)
{
	ASSERT(_body_handle);
	dBodySetRotation(_body_handle, reinterpret_cast<Scalar const *>(matrix.GetArray()));
}
Пример #27
0
	void OpenGLContext::setIdentity() {
		Matrix33* active = getActiveMatrix();
		active->setIdentity();
		loadGLMatrix();
	}
Пример #28
0
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_debugFreezeShake)
			{
				static float color[] = {1,1,1,1};
				gEnv->pRenderer->Draw2dLabel(100,200,1.5,color,false,"limit: %f", RAD2DEG(frozenLimit));
			}
		}

		if(m_player.m_stats.isOnLadder)
		{
			limitDir = -m_player.m_stats.ladderOrientation;
			limitH = DEG2RAD(40.0f);
		}

		if((limitH+limitV+limitVUp+limitVDown) && limitDir.len2()>0.1f)
		{
			//A matrix is built around the view limit, and then the player view angles are checked with it.
			//Later, if necessary the upVector could be made customizable.
			Vec3 forward(limitDir);
			Vec3 up(m_baseQuat.GetColumn2());
			Vec3 right(-(up % forward));
			right.Normalize();

			Matrix33 limitMtx;
			limitMtx.SetFromVectors(right,forward,right%forward);
			//gEnv->pRenderer->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(0,0,255,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(0), ColorB(0,0,255,255));
			//gEnv->pRenderer->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(0,255,0,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(1), ColorB(0,255,0,255));
			//gEnv->pRenderer->GetIRenderAuxGeom()->DrawLine(m_player.GetEntity()->GetWorldPos(), ColorB(255,0,0,255), m_player.GetEntity()->GetWorldPos() + limitMtx.GetColumn(2), ColorB(255,0,0,255));
			limitMtx.Invert();

			Vec3 localDir(limitMtx * m_viewQuat.GetColumn1());
//			Vec3 localDir(limitMtx * m_player.GetEntity()->GetWorldRotation().GetColumn1());

			Ang3 limit;

			if(limitV)
			{
				limit.x = asinf(localDir.z) + m_deltaAngles.x;

				float deltaX(limitV - fabs(limit.x));

				if(deltaX < 0.0f)
					m_deltaAngles.x += deltaX*(limit.x>0.0f?1.0f:-1.0f);
			}

			if(limitVUp || limitVDown)
			{
				limit.x = asinf(localDir.z) + m_deltaAngles.x;

				if(limit.x>=limitVUp && limitVUp!=0)
				{
					float deltaXUp(limitVUp - limit.x);
					m_deltaAngles.x += deltaXUp;
				}

				if(limit.x<=limitVDown && limitVDown!=0)
				{
					float deltaXDown(limitVDown - limit.x);
					m_deltaAngles.x += deltaXDown;
				}
			}

			if(limitH)
			{
				limit.z = cry_atan2f(-localDir.x,localDir.y) + m_deltaAngles.z;

				float deltaZ(limitH - fabs(limit.z));

				if(deltaZ < 0.0f)
					m_deltaAngles.z += deltaZ*(limit.z>0.0f?1.0f:-1.0f);
			}
		}
	}
}
int main(int arg, char* argc[]) {

  DetermineCameraPositionParser parser(helpMsg);

  try{

    parser.parse(arg,argc);
  
    MarkerType markerType;
    vector<Point2d> markers;
    string dataFolder = parser.get<string>("dataFolder");
    int camID = parser.get<int>("camID");
    int camW = parser.get<int>("camWidth");
    int camH = parser.get<int>("camHeight");

    string markerName = dataFolder + "/markers/" + parser.get<string>("markerName") + ".txt";
    markerType =  MarkerType(markerName);
  
    string distanceFile = dataFolder + "/distances/" + parser.get<string>("distanceMatrix") + ".txt";
    MatrixDD distances = loadDistanceMatrix(distanceFile);
  
    Mat src;

    string cameraFileName = dataFolder + "/camera/" + parser.get<string>("cameraName") + ".txt";
    Camera cam = loadCameraFromFile( cameraFileName );

    VideoCapture cap(camID);
    cap.set(CV_CAP_PROP_FRAME_WIDTH,cam.getResWidth());
    cap.set(CV_CAP_PROP_FRAME_HEIGHT,cam.getResHeight());

    if (!cap.isOpened())
      return false;

    Matrix33 planeR = Matrix33::Identity();
    Vector3 planeT;
  
    while (1) {
      cap >> src;

      // Track markers
      trackMarker(src, markerType, markers);

      // sort markers in circular patern
      reorganizePoints(markers);

      // print markers in picture
      for (size_t i = 0; i < markers.size(); i++) {
	circle(src, markers[i], 1, Scalar(255, 0, 0), 2);
	stringstream ss;
	ss << i;
	putText(src, ss.str(), markers[i], FONT_HERSHEY_PLAIN, 1.5,
		Scalar(255, 0, 0));
      }

      if (markers.size() == distances.rows() ) {

	vector<Vector3> outPoints;
	double optiError;
	
	vector<Real> x(distances.rows(),2000);
	optiError = findPointPositions(distances, markers, cam, outPoints, x);

	findPlaneCoordinateFrame( cam, outPoints, planeR, planeT );

	cout << "Camera is: " << (planeR.transpose()*(cam.getT() - planeT))[2] << "mm above ground." << endl;
	cout << "Marker distances are: ";
	for(Real y : x){
	  cout << y << " ";
	}
	cout << endl << endl;

      }else{
	cout << "Detected " << markers.size() << " ground markers but there should be " << distances.rows() << " markers in the picture!" << endl;
      }

      imshow("determineCameraPosition", src);
     
      if (waitKey(1) >= 0)
	break;
    }
  }catch( std::exception const & exc ){
    cerr << exc.what() << endl;
    return 0;
  }

  return 0;
}
void CCameraInputHelper::UpdateCameraInput(Ang3 &deltaRotation, const float frameTimeClamped, const float frameTimeNormalised)
{
	// Use new control scheme,
	// or, if we're just entering/leaving it then adjust camera/character orientations as appropriate.

	//--- Translate left stick into a deltaRotation.z (and a move direction)

	Vec3 vcStick=Vec3(m_pHeroInput->m_moveStickLR, m_pHeroInput->m_moveStickUD,0);	// Stick vector in view space	// -m_moveStickLR removes flicker, but inverses direction..

	static Vec3 vcStickPrev=Vec3(0,1,0);

	float stickMag=vcStick.len();

	if(stickMag>0.001f)
		vcStick*=1/stickMag;
	else
		vcStick=vcStickPrev;	// should hold previous value/direction unless we're in transition.

	vcStickPrev=vcStick;

	Matrix33 camViewMtx = m_pHero->m_camViewMtxFinal;
	//in flight mode the actual camera matrix changes
	//the mtxFinal itself stays untouched, so that the camera can move back after the flight
	if(CCameraFlight::GetInstance()->IsCameraFlightActive())
	{
		Vec3 vCamDir = CCameraFlight::GetInstance()->GetLookingDirection();
		camViewMtx.SetRotationVDir(vCamDir, 0.0f);
	}
	
	const Matrix34& entityWorldTM = m_pHero->GetEntity()->GetWorldTM();
	Vec3 vwEntPos = entityWorldTM.GetTranslation();
	Vec3 vwDrawFrom = vwEntPos+Vec3(0,0,0.8f);
	Vec3 vwEntityFront = entityWorldTM.GetColumn1();
	Vec3 vwEntityRight = entityWorldTM.GetColumn0();
	Vec3 vCamRef = camViewMtx*Vec3(0,1,0);
	Vec3 vwStick = camViewMtx*vcStick;	// Stick vector in world space
	Vec3 vwfStick = Vec3FlattenXY(vwStick);

	Vec3 vwfEntityFront=Vec3FlattenXY(vwEntityFront);
	Vec3 vwfEntityRight=Vec3FlattenXY(vwEntityRight);
	float dotRight=vwfStick.Dot(vwfEntityRight);
	float dotFront=vwfStick.Dot(vwfEntityFront);
	dotFront = clamp(dotFront,-1.0f,1.0f);

	bool enteredNavFromCombat = false; //(m_bNavCombatModeChanged && !bIsCombatMode);

	//--- Just moved into nav mode from combat
	if(enteredNavFromCombat)
	{
		// so set directions appropriately.

		float dotFront2=(vCamRef).Dot(Vec3(0,1,0));
		float dotRight2=(camViewMtx*Vec3(1,0,0)).Dot(Vec3(0,1,0));
		float yaw = 0.0f;

		dotFront2 = clamp(dotFront2,-1.0f,1.0f);
		if(dotRight2>=0)
		{
			yaw=-cry_acosf(dotFront2);
		}
		else
		{
			yaw=cry_acosf(dotFront2);
		}

		m_fYaw = yaw;
	}

	//this is the actual entity rotation (yaw movement)
	CCameraOverrides *pCamOverride = g_pGame->GetCameraManager()->GetCamOverrides();
	int eCamOverrideMode = pCamOverride->GetCameraOverride();

	//the ECO_LOWCOVER fading is a hack, causing problems when inputs are changed during the fade
	//in combat mode all inputs have to be passed through to synchronize the camera direction in other modes
	if((stickMag>0.01f && eCamOverrideMode != ECO_LOWCOVER))
	{
		if(dotRight>=0)
		{
			deltaRotation.z=-cry_acosf(dotFront);
		}
		else
		{
			deltaRotation.z=cry_acosf(dotFront);
		}
	}
	else
		deltaRotation.z=0;

	float maxDeltaRot = 7.85f; // safety value //g_pGameCVars->cl_nav_SprintMaxTurnRate;
	Limit( maxDeltaRot, 0.f, gf_PI * 10.f ); // limit to 3600 degrees per second, i.e. 'pretty fast'
	maxDeltaRot *= frameTimeClamped;	

	// In Nav mode movement is always in the characters forward direction.
	UpdatePitchYawDriver(stickMag, frameTimeNormalised);

	m_bNavCombatModeChanged=false;
}