C++ Vec3::Cross方法代码示例

	//----------------------------------------------------------------------------
	void ProcessInputData()
	{
		if (mOverridingCamera){
			mOverridingCamera->ProcessInputData();
			return;
		}
		auto target = mTarget.lock();
		if (!mProcessInput || !mCurrentCamera || !target)
			return;
		if (mUserParams.Changed() || mPrevTargetPos != target->GetPosition())
		{
			mInternalParams.dist += mUserParams.dDist;
			mInternalParams.dist = std::max((Real)2.0f, (Real)mInternalParams.dist);
			if (mInternalParams.dist > 300.0)
				mInternalParams.dist = 300.0;

			mInternalParams.pitch += mUserParams.dPitch;
			if (mInternalParams.pitch > fb::HALF_PI - fb::Radian(5))
			{
				mInternalParams.pitch = fb::HALF_PI - fb::Radian(5);
			}
			else if (mInternalParams.pitch <  -fb::HALF_PI + fb::Radian(5))
			{
				mInternalParams.pitch = -fb::HALF_PI + fb::Radian(5);
			}

			mInternalParams.yaw += mUserParams.dYaw;
			if (mInternalParams.yaw > fb::TWO_PI)
			{
				mInternalParams.yaw -= fb::TWO_PI;
			}
			else if (mInternalParams.yaw < -fb::TWO_PI)
			{
				mInternalParams.yaw += fb::TWO_PI;
			}

			Vec3 defaultDir = -Vec3::UNIT_Y;
			Quat qPitch(mInternalParams.pitch, Vec3::UNIT_X);
			Quat qYaw(-mInternalParams.yaw, Vec3::UNIT_Z);
			Vec3 toCam = qPitch * defaultDir;
			toCam = qYaw * toCam;
			Vec3 forward = -toCam;
			Vec3 right = forward.Cross(Vec3::UNIT_Z);
			right.Normalize();
			Vec3 up = right.Cross(forward);
			forward = up.Cross(right);

			Mat33 rot(right.x, forward.x, up.x,
				right.y, forward.y, up.y,
				right.z, forward.z, up.z);
			Vec3 pos = target->GetPosition() + toCam * mInternalParams.dist;
			SetTransformation(pos, Quat(rot));
			mUserParams.Clear();
			mPrevTargetPos = target->GetPosition();
		}
	}

    static Transform LookAtRH(const Vec3 eye, const Vec3 at, const Vec3 up)
    {
        Vec3 n = (eye - at).Normalized();
        Vec3 u = up.Cross(n).Normalized();
        Vec3 v = n.Cross(u);

        float w = sqrtf(1.0f + u.x + v.y + n.z) * 0.5f;
        float w4 = 1.0f / (4.0f * w);
        
        return Transform(   Vec3(-u.Dot(eye), -v.Dot(eye), -n.Dot(eye)), // Position
                            Quat((v.z-n.y) * w4, (n.x-u.z) * w4, (u.y-v.x) * w4, -w).Normalized(), // Rotation
                            1); // Scale

    }

void CFlashUIWorldScreenPosNode::GetScreenPosFromWorld(IUIElement* pInstance, Vec3& pos /*in/out*/, Vec3& offset /*in/out*/, bool bScaleMode)
{
    // get current camera matrix
    const CCamera& cam = GetISystem()->GetViewCamera();
    const Matrix34& camMat = cam.GetMatrix();

    // add offset to position
    const Vec3 vFaceingPos = camMat.GetTranslation() - camMat.GetColumn1() * 1000.f;
    const Vec3 vDir = (pos - vFaceingPos).GetNormalizedFast();
    const Vec3 vOffsetX = vDir.Cross(Vec3Constants<float>::fVec3_OneZ).GetNormalizedFast() * offset.x;
    const Vec3 vOffsetY = vDir * offset.y;
    const Vec3 vOffsetZ = Vec3(0, 0, offset.z);
    const Vec3 vOffset = vOffsetX + vOffsetY + vOffsetZ;
    pos += vOffset;

    Vec2 borders;
    float scale;
    Vec3 flashPos;
    pInstance->WorldToFlash(camMat, pos, flashPos, borders, scale, bScaleMode);

    // return flashpos in pos
    pos = flashPos;

    // store overflow in offset x/y and scale in z
    offset.x = borders.x;
    offset.y = borders.y;
    offset.z = scale;
}

void CDeflectorShield::ShootDeflectedEnergy(const CDeflectorShield::SDeflectedEnergy& energy)
{
	if (!m_pAmmoClass)
		return;
	CProjectile* pEnergyBlast = g_pGame->GetWeaponSystem()->SpawnAmmo(m_pAmmoClass, false);
	if (!pEnergyBlast)
		return;

	const Matrix34& worldTransform = GetEntity()->GetWorldTM();

	const float positionBias = 0.05f;

	const Vec3 worldReflectPos = worldTransform.TransformPoint(energy.m_localPosition);
	const Vec3 worldReflectDir = worldTransform.TransformVector(energy.m_localDirection);

	const Vec3 worldSpreadU = worldReflectDir.GetOrthogonal();
	const Vec3 worldSpreadV = worldReflectDir.Cross(worldSpreadU);

	const float spreadOffset = cry_random(0.0f, m_spread);
	
	const Vec3 position = worldReflectPos + worldReflectDir * positionBias;
	Vec3 direction = 
		worldReflectDir + 
		(worldSpreadU * cry_random(0.0f, m_spread)) + 
		(worldSpreadV * cry_random(0.0f, m_spread));
	direction.Normalize();

	CProjectile::SProjectileDesc projectileDesc(
		0, 0, 0, energy.m_damage, m_dropMinDistance, m_dropPerMeter, float(m_minDamage), m_hitTypeId,
		0, false);
	pEnergyBlast->SetParams(projectileDesc);
	pEnergyBlast->Launch(position, direction, Vec3(ZERO));
}

    void UnderWaterGodRay::_updateProjector()
    {
        Plane p(Vec3(0, 1, 0), mWaterPosition);
        Vec3 sunDir = Environment::Instance()->GetEvParam()->SunDir;
        Vec3 sunPos = Environment::Instance()->GetEvParam()->SunPos;
        Ray r(sunPos, sunDir);

        RayIntersectionInfo info = r.Intersection(p);

        mEnable = info.iterscetion;

        mProjPosition = sunPos + sunDir * info.distance;

        Vec3 y(0, 1, 0);

        if (sunDir.Dot(y) < 0.001)
            y = Vec3(0, 0, -1);

        Vec3 x = y.Cross(sunDir);
        y = sunDir.Cross(x);
        
        mProjMatrix.MakeRotationAxis(x, y, sunDir);
        mProjMatrix._41 = mProjPosition.x;
        mProjMatrix._42 = mProjPosition.y;
        mProjMatrix._43 = mProjPosition.z;
    }

inline float AngleBetweenVectors( const Vec3 &v1,const Vec3 &v2 )
{
	float a = acos_tpl(v1.Dot(v2));
	Vec3 r = v1.Cross(v2);
	if (r.z < 0)
		a = -a;
	return a;
}

Ang3 CVehicleSeatActionOrientateBoneToView::GetDesiredViewAngles(const Vec3& lookPos, const Vec3& aimPos) const
{
	Vec3 forwardDir = (aimPos - lookPos).GetNormalized();
	Vec3 upDir = Vec3(0.f, 0.f, 1.f);
	Vec3 sideDir = forwardDir.Cross(upDir);
	sideDir.Normalize();
	upDir = sideDir.Cross(forwardDir);
	upDir.Normalize();

	Matrix34 matrix;
	matrix.SetFromVectors(sideDir, forwardDir, upDir, Vec3(0.f, 0.f, 0.f));

	Ang3 lookAngles;
	lookAngles.SetAnglesXYZ(matrix);

	return lookAngles;
}

// Ad-hoc
void CFlyer::ProcessMovement(float frameTime)
{
	frameTime = min(1.f, frameTime);

	float desiredSpeed = m_vDesiredVelocity.GetLength();
	const float maxDeltaSpeed = 100.f;

	float deltaSpeed = min(maxDeltaSpeed, fabsf(desiredSpeed - m_stats.speed));

	// Advance "m_velocity" towards "m_vDesiredVelocity" at the speed proportional to (1 / square(deltaSpeed))
	Interpolate(
		m_velocity,
		m_vDesiredVelocity,
		2.5f * ((deltaSpeed > 0.f) ? min(frameTime, 2.f / square(deltaSpeed)) : frameTime),
		1.f);

	Quat desiredVelocityQuat = m_qDesiredRotation;

	// pitch/roll
	if (desiredSpeed > 0.f && m_stats.speed > 0.f)
	{
		const Vec3& vUp = Vec3Constants<float>::fVec3_OneZ;
		Vec3 vForward = m_velocity.GetNormalized();

		// If the direction is not too vertical
		if (fabs(vForward.dot(vUp)) < cosf(DEG2RAD(3.f)))
		{
			vForward.z = 0;
			vForward.NormalizeSafe();
			Vec3 vRight = vForward.Cross(vUp);
			vRight.NormalizeSafe();

			Vec3 vDesiredVelocityNormalized = m_vDesiredVelocity.GetNormalized();

			// Roll in an aircraft-like manner
			float cofRoll = 6.f * vRight.dot(vDesiredVelocityNormalized) * (m_stats.speed / maxDeltaSpeed);
			clamp(cofRoll, -1.f, 1.f);
			desiredVelocityQuat *= Quat::CreateRotationY(DEG2RAD(60.f) * cofRoll); 

			float cofPitch = vDesiredVelocityNormalized.dot(vForward) * (deltaSpeed / maxDeltaSpeed);
			clamp(cofPitch, -1.f, 1.f);
			desiredVelocityQuat *= Quat::CreateRotationX(DEG2RAD(-60.f) * cofPitch); 
		}
	}

	float cofRot = 2.5f * ((deltaSpeed > 0.f) ? min(frameTime, 1.f / square(deltaSpeed)) : frameTime);
	clamp(cofRot, 0.f, 1.f);
	const Quat& qRotation = GetEntity()->GetRotation();
	Quat newRot = Quat::CreateSlerp(qRotation, desiredVelocityQuat, cofRot);
	m_moveRequest.rotation = qRotation.GetInverted() * newRot;
	m_moveRequest.rotation.Normalize();

	m_moveRequest.velocity = m_velocity;

	m_moveRequest.type = eCMT_Fly;
}

void CVehicleMovementAerodynamic::UpdateWing(SWing *_pWing,float _fAngle,float _fDeltaTime)
{
	Matrix34 matWing = m_pEntity->GetWorldTM() * Matrix33::CreateRotationXYZ(Ang3(DEG2RAD(_pWing->fAngleX),
																																								DEG2RAD(_pWing->fAngleY),
																																								DEG2RAD(_pWing->fAngleZ))).GetInverted();

	Vec3 vRight	= matWing.GetColumn(0);
	Vec3 vLook	= matWing.GetColumn(1);
	Vec3 vUp		= matWing.GetColumn(2);

	pe_status_dynamics StatusDynamics;
	GetPhysics()->GetStatus(&StatusDynamics);

	// v(relativepoint) = v + w^(relativepoint-center)
	Vec3 vVelocity = StatusDynamics.v + StatusDynamics.w.Cross(m_pEntity->GetWorldTM().TransformVector(_pWing->vPos));
	Vec3 vVelocityNormalized = vVelocity.GetNormalizedSafe(vLook);

	// TODO:

	float fAngleOfAttack = RAD2DEG(asin(vRight.Dot(vVelocityNormalized.Cross(vLook))));

	DumpText("AoA=%f",fAngleOfAttack);

	fAngleOfAttack += _fAngle;

	float Cl = GetCoefficient(_pWing->pLiftPointsMap,fAngleOfAttack) * _pWing->fCl;
	float Cd = GetCoefficient(_pWing->pDragPointsMap,fAngleOfAttack) * _pWing->fCd;

	Vec3 vVelocityNormal = vRight.Cross(vVelocityNormalized).GetNormalized();

	float fVelocitySquared = vVelocity.len2();

	const float c_fDynamicPressure = 1.293f;

	float fLift = 0.5f * c_fDynamicPressure * _pWing->fSurface * Cl * fVelocitySquared * _fDeltaTime;
	float fDrag = 0.5f * c_fDynamicPressure * _pWing->fSurface * Cd * fVelocitySquared * _fDeltaTime;

	Vec3 vLiftImpulse = +fLift * vVelocityNormal;
	Vec3 vDragImpulse = -fDrag * vVelocityNormalized;

	AddForce(&vLiftImpulse,&_pWing->vPos,ColorF(0,1,0,1));
	AddForce(&vDragImpulse,&_pWing->vPos,ColorF(1,0,1,1));
}

Matrix Matrix::LookAt(const Vec3& position, const Vec3& target, const Vec3& up) {
  /// Generate view matrix for position camera 	
  /// http://wiki.delphigl.com/index.php/gluLookAt
  const float epsilon = 0.001f;

  Vec3 f = -(target - position).Normal();
  Vec3 s = -f.Cross(up.Normal());
  float sl = s.Length();
  if (fabs(sl) < epsilon) {
    s = -f.Cross(Vec3(up.y, up.z, up.x).Normal());
    sl = s.Length();
  }
  s /= sl;
  Vec3 u = s.Cross(f);

  Matrix m;
  m.m[0] = s.x;	  m.m[1] = s.y;	  m.m[2] = s.z;	  m.m[3] = 0;
  m.m[4] = u.x;	  m.m[5] = u.y;	  m.m[6] = u.z;	  m.m[7] = 0;
  m.m[8] = -f.x;	m.m[9] = -f.y;	m.m[10] = -f.z;	m.m[11] = 0;
  m.m[12] = 0;		m.m[13] = 0;		m.m[14] = 0;		m.m[15] = 1;
  return Translate(-position) * m;
}

Vec3 RapidTrajectoryGenerator::GetOmega(double t, double timeStep) const
{
  //Calculates the body rates necessary at time t, to rotate the normal vector.
  //The result is coordinated in the world frame, i.e. would have to be rotated into a
  //body frame.
	const Vec3 n0 = GetNormalVector(t);
	const Vec3 n1 = GetNormalVector(t + timeStep);

	const Vec3 crossProd = n0.Cross(n1); //direction of omega, in inertial axes

	if (crossProd.GetNorm2()) return  (acos(n0.Dot(n1))/timeStep)*crossProd.GetUnitVector();
	else return Vec3(0, 0, 0);
}

        inline Vec3 RotatedBy(Vec3 const &axisVec, double angleDegCCW)
        {
            if(!angleDegCCW)
            {   return Vec3(this->x,this->y,this->z);   }

            Vec3 rotatedVec;
            double angleRad = angleDegCCW*3.141592653589/180.0;
            rotatedVec = this->ScaledBy(cos(angleRad)) +
                         (axisVec.Cross(*this)).ScaledBy(sin(angleRad)) +
                         axisVec.ScaledBy(axisVec.Dot(*this)).ScaledBy(1-cos(angleRad));

            return rotatedVec;
        }

// MakeLookAt
//------------------------------------------------------------------------------
void Mat44::MakeLookAt( const Vec3 & pos, const Vec3 & lookAt, const Vec3 & upVector )
{
    // generate the forward vector
    Vec3 forward( pos - lookAt );
    forward.Normalise();

    Vec3 right = upVector.Cross( forward );
    right.Normalise();

    Vec3 up = forward.Cross( right );
    up.Normalise();

    col0 = Vec4( right.x, up.x, forward.x, 0.0f );
    col1 = Vec4( right.y, up.y, forward.y, 0.0f ),
    col2 = Vec4( right.z, up.z, forward.z, 0.0f ),
    col3 = Vec4( -right.Dot( pos ), -up.Dot( pos ), -forward.Dot( pos ), 1.0f );
}

void CStickyProjectile::CalculateLocationForStick( const IEntity& projectile, const Vec3& collPos, const Vec3& collNormal, QuatT& outLocation ) const
{
	if(m_flags&eSF_OrientateToCollNormal)
	{
		const float bigz = cry_fabsf(collNormal.z)-cry_fabsf(collNormal.y);
		const Vec3 temp(0.f,(float)__fsel(bigz,1.f,0.f),(float)__fsel(bigz,0.f,1.f));
		outLocation.q = Quat(Matrix33::CreateOrientation( temp.Cross(collNormal), -collNormal, 0));

		AABB aabb;
		projectile.GetLocalBounds(aabb);
		outLocation.t = collPos + (outLocation.q.GetColumn2() * ((aabb.max.y-aabb.min.y)*0.5f));
	}
	else
	{
		outLocation.q = projectile.GetRotation();
		outLocation.t = collPos + (outLocation.q.GetColumn1() * 0.1f);
	}
}

void CCameraRayScan::ShootRays(const Vec3 &rayPos, const Vec3 &rayDir, IPhysicalEntity **pSkipEnts, int numSkipEnts)
{

	//shoot rays for all ray_hits
	const Vec3 dirNorm = rayDir.normalized();
	const Vec3 right = dirNorm.Cross(Vec3Constants<float>::fVec3_OneZ);
	const Vec3 rightOff = right * 0.15f;
	const Vec3 rightDir = right * 0.15f;
	const float len = rayDir.len() * RAY_SCAN_BUFFER_SCALE; //add some distance to be sure that the view is free

	const Vec3 rayPos2 = rayPos + (dirNorm * RAY_SCAN_OFFSET_DISTANCE); //move the rays away from the head to prevent clipping

	//center ray
	Vec3 tempPos = rayPos2;
	Vec3 tempDir = dirNorm;
	ShootRayInt(eRAY_CENTER, tempPos, tempDir, len, pSkipEnts, numSkipEnts);

	tempDir = (dirNorm - rightDir + g_vRayUpDir).normalized();
	tempPos = rayPos2 - rightOff + g_vRayUpOffset;
	ShootRayInt(eRAY_TOP_LEFT, tempPos, tempDir, len, pSkipEnts, numSkipEnts);

	tempDir = (dirNorm + rightDir + g_vRayUpDir).normalized();
	tempPos = rayPos2 + rightOff + g_vRayUpOffset;
	ShootRayInt(eRAY_TOP_RIGHT, tempPos, tempDir, len, pSkipEnts, numSkipEnts);

	tempDir = (dirNorm - rightDir - g_vRayUpDir).normalized();
	tempPos = rayPos2 - rightOff - g_vRayUpOffset;
	ShootRayInt(eRAY_BOTTOM_LEFT, tempPos, tempDir, len, pSkipEnts, numSkipEnts);

	tempDir = (dirNorm + rightDir - g_vRayUpDir).normalized();
	tempPos = rayPos2 + rightOff - g_vRayUpOffset;
	ShootRayInt(eRAY_BOTTOM_RIGHT, tempPos, tempDir, len, pSkipEnts, numSkipEnts);

	tempDir = (dirNorm + g_vRayUpDir).normalized();
	tempPos = rayPos2 + g_vRayUpOffset * 2.0f;
	ShootRayInt(eRAY_TOP_CENTER, tempPos, tempDir, len, pSkipEnts, numSkipEnts);

	tempDir = (dirNorm - g_vRayUpDir).normalized();
	tempPos = rayPos2 - g_vRayUpOffset * 2.0f;
	ShootRayInt(eRAY_BOTTOM_CENTER, tempPos, tempDir, len, pSkipEnts, numSkipEnts);
}