C++ Vector3d::dotProduct方法代码示例

// Find the closest point on the walkplane to the given point
Math::Vector3d Sector::getClosestPoint(const Math::Vector3d &point) const {
	// First try to project to the plane
	Math::Vector3d p2 = getProjectionToPlane(point);
	if (isPointInSector(p2))
		return p2;

	// Now try to project to some edge
	for (int i = 0; i < _numVertices; i++) {
		Math::Vector3d edge = _vertices[i + 1] - _vertices[i];
		Math::Vector3d delta = point - _vertices[i];
		float scalar = Math::Vector3d::dotProduct(delta, edge) / Math::Vector3d::dotProduct(edge, edge);
		Math::Vector3d cross = Math::Vector3d::crossProduct(delta, edge);
		if (scalar >= 0 && scalar <= 1 && cross.dotProduct(_normal) > 0)
			// That last test is just whether the z-component
			// of delta cross edge is positive; we don't
			// want to return opposite edges.
			return _vertices[i] + scalar * edge;
	}

	// Otherwise, just find the closest vertex
	float minDist = (point - _vertices[0]).getMagnitude();
	int index = 0;
	for (int i = 1; i < _numVertices; i++) {
		float currDist = (point - _vertices[i]).getMagnitude();
		if (currDist < minDist) {
			minDist = currDist;
			index = i;
		}
	}
	return _vertices[index];
}

void Sector::getExitInfo(const Math::Vector3d &s, const Math::Vector3d &dirVec, struct ExitInfo *result) const {
	Math::Vector3d start = getProjectionToPlane(s);
	Math::Vector3d dir = getProjectionToPuckVector(dirVec);

	// First find the edge the ray exits through: this is where
	// the z-component of (v_i - start) x dir changes sign from
	// positive to negative.

	// First find a vertex such that the cross product has
	// positive z-component.
	int i;
	for (i = 0; i < _numVertices; i++) {
		Math::Vector3d delta = _vertices[i] - start;
		Math::Vector3d cross = Math::Vector3d::crossProduct(delta, dir);
		if (cross.dotProduct(_normal) > 0)
			break;
	}

	// Now continue until the cross product has negative
	// z-component.
	while (i < _numVertices) {
		i++;
		Math::Vector3d delta = _vertices[i] - start;
		Math::Vector3d cross = Math::Vector3d::crossProduct(delta, dir);
		if (cross.dotProduct(_normal) <= 0)
			break;
	}

	result->edgeDir = _vertices[i] - _vertices[i - 1];
	result->angleWithEdge = Math::Vector3d::angle(dir, result->edgeDir);
	result->edgeVertex = i - 1;

	Math::Vector3d edgeNormal = Math::Vector3d::crossProduct(result->edgeDir, _normal);
	float d = Math::Vector3d::dotProduct(dir, edgeNormal);
	// This is 0 for the albinizod monster in the at set
	if (!d)
		d = 1.f;
	result->exitPoint = start + (Math::Vector3d::dotProduct(_vertices[i] - start, edgeNormal) / d) * dir;
}

bool Sector::isPointInSector(const Math::Vector3d &point) const {
	// Calculate the distance of the point from the plane of the sector.
	// Return false if it isn't within a margin.
	if (_height < 9000.f) { // No need to check when height is 9999.

		float dist = distanceToPoint(point);

		if (fabsf(dist) > _height + 0.01) // Add an error margin
			return false;
	}

	// On the plane, so check if it is inside the polygon.
	for (int i = 0; i < _numVertices; i++) {
		Math::Vector3d edge = _vertices[i + 1] - _vertices[i];
		Math::Vector3d delta = point - _vertices[i];
		Math::Vector3d cross = Math::Vector3d::crossProduct(edge, delta);
		if (cross.dotProduct(_normal) < -0.000001f) // not "< 0.f" here, since the value could be something like -7.45058e-09 and it
			return false;                        // shuoldn't return. that was causing issue #610 (infinite loop in de.forklift_actor.dismount)
	}
	return true;
}