C++ vec3::Dot方法代码示例

bool RayIntersectsPlane(const ray& r, const vec3& p, const vec3& n, vec3* hit = NULL)
{
	vec3 w0 = r.m_pos - p;

	float a = -n.Dot(w0);
	float b = n.Dot(r.m_dir);

	float ep = 1e-4f;

	if (fabs(b) < ep)
	{
		if (a == 0)			// Ray is parallel
			return false;	// Ray is inside plane
		else
			return false;	// Ray is somewhere else
	}

	float r = a/b;
	if (r < 0)
		return false;		// Ray goes away from the plane

	vec3 vecPoint = r.m_pos + r.m_dir*r;
	if (hit)
		*hit = vecPoint;

	return true;
}

float DistanceToPlaneSqr(const vec3& vecPoint, const vec3& vecPlane, const vec3& vecPlaneNormal)
{
	float sb, sn, sd;

	sn = -vecPlaneNormal.Dot(vecPoint - vecPlane);
	sd = vecPlaneNormal.Dot(vecPlaneNormal);
	sb = sn/sd;

	vec3 b = vecPoint + vecPlaneNormal * sb;
	return (vecPoint - b).LengthSqr();
}

	const Quaternion Quaternion::Rotation(const vec3& unitVec0, const vec3& unitVec1)
	{
		float cosHalfAngleX2, recipCosHalfAngleX2;
		cosHalfAngleX2 = sqrt((2.0f * (1.0f + unitVec0.Dot(unitVec1))));
		recipCosHalfAngleX2 = (1.0f / cosHalfAngleX2);
		return Quaternion((unitVec0.Cross(unitVec1) * recipCosHalfAngleX2), (cosHalfAngleX2 * 0.5f));
	}

	static Matrix4<T> LookAtRH(vec3 eye, vec3 at, vec3 up)
	{
		vec3 viewDir = at-eye;
		viewDir.Normalize();

		vec3 side = viewDir.Cross(up);
		side.Normalize();

		vec3 newUp = side.Cross(viewDir);
		newUp.Normalize();

		Matrix4 m;
		m.x.x = side.x;		m.y.x = side.y;		m.z.x = side.z;		m.w.x = -eye.Dot(side);
		m.x.y = newUp.x;	m.y.y = newUp.y;	m.z.y = newUp.z;	m.w.y = -eye.Dot(newUp);
		m.x.z = -viewDir.x;	m.y.z = -viewDir.y;	m.z.z = -viewDir.z;	m.w.z = eye.Dot(viewDir);
		m.x.w = 0.0f;		m.y.w = 0.0f;		m.z.w = 0.0f;		m.w.w = 1.0f;
		return m;
	}

	static CQuaternion CreateFromVectors(vec3 v0, vec3 v1)  {
		CQuaternion q;
		
		if (v0 == -v1) {//Create from Axis Angle
		  //int a = 1;
		}

		vec3 c = v0.Cross(v1);
		float d = v0.Dot(v1);
		
		float s = (float)  sqrt((1+d)*2);

		q.x = c.x /s;
		q.y = c.y/s;
		q.z = c.z/s;
		q.w = s / 2.0f;
		return q;
	}

bool LineSegmentIntersectsSphere(const vec3& v1, const vec3& v2, const vec3& s, float flRadius, vec3& vecPoint, vec3& vecNormal)
{
	vec3 vecLine = v2 - v1;
	vec3 vecSphere = v1 - s;

	if (vecLine.LengthSqr() == 0)
	{
		if (vecSphere.LengthSqr() < flRadius*flRadius)
		{
			vecPoint = v1;
			vecNormal = vecSphere.Normalized();
			return true;
		}
		else
			return false;
	}

	float flA = vecLine.LengthSqr();
	float flB = 2 * vecSphere.Dot(vecLine);
	float flC1 = s.LengthSqr() + v1.LengthSqr();
	float flC2 = (s.Dot(v1)*2);
	float flC = flC1 - flC2 - flRadius*flRadius;

	float flBB4AC = flB*flB - 4*flA*flC;
	if (flBB4AC < 0)
		return false;

	float flSqrt = sqrt(flBB4AC);
	float flPlus = (-flB + flSqrt)/(2*flA);
	float flMinus = (-flB - flSqrt)/(2*flA);

	bool bPlusBelow0 = flPlus < 0;
	bool bMinusBelow0 = flMinus < 0;
	bool bPlusAbove1 = flPlus > 1;
	bool bMinusAbove1 = flMinus > 1;

	// If both are above 1 or below 0, then we're not touching the sphere.
	if (bMinusBelow0 && bPlusBelow0 || bPlusAbove1 && bMinusAbove1)
		return false;

	if (bMinusBelow0 && bPlusAbove1)
	{
		// We are inside the sphere.
		vecPoint = v1;
		vecNormal = (v1 - s).Normalized();
		return true;
	}

	if (bMinusAbove1 && bPlusBelow0)
	{
		// We are inside the sphere. Is this even possible? I dunno. I'm putting an assert here to see.
		// If it's still here later that means no.
		TAssert(false);
		vecPoint = v1;
		vecNormal = (v1 - s).Normalized();
		return true;
	}

	// If flPlus is below 1 and flMinus is below 0 that means we started our trace inside the sphere and we're heading out.
	// Don't intersect with the sphere in this case so that things on the inside can get out without getting stuck.
	if (bMinusBelow0 && !bPlusAbove1)
		return false;

	// So at this point, flMinus is between 0 and 1, and flPlus is above 1.
	// In any other case, we intersect with the sphere and we use the flMinus value as the intersection point.
	float flDistance = vecLine.Length();
	vec3 vecDirection = vecLine / flDistance;

	vecPoint = v1 + vecDirection * (flDistance * flMinus);

	// Oftentimes we are slightly stuck inside the sphere. Pull us out a little bit.
	vec3 vecDifference = vecPoint - s;
	float flDifferenceLength = vecDifference.Length();
	vecNormal = vecDifference / flDifferenceLength;
	if (flDifferenceLength < flRadius)
		vecPoint += vecNormal * ((flRadius - flDifferenceLength) + 0.00001f);
	TAssert((vecPoint - s).LengthSqr() >= flRadius*flRadius);

	return true;
}