db5ff8ec4a
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@392 92aac97c-a6ce-11dd-a772-7fcde58d38e6
451 lines
22 KiB
C++
451 lines
22 KiB
C++
/********************************************************************************
|
|
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
|
* Copyright (c) 2010 Daniel Chappuis *
|
|
*********************************************************************************
|
|
* *
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy *
|
|
* of this software and associated documentation files (the "Software"), to deal *
|
|
* in the Software without restriction, including without limitation the rights *
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
|
|
* copies of the Software, and to permit persons to whom the Software is *
|
|
* furnished to do so, subject to the following conditions: *
|
|
* *
|
|
* The above copyright notice and this permission notice shall be included in *
|
|
* all copies or substantial portions of the Software. *
|
|
* *
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
|
|
* THE SOFTWARE. *
|
|
********************************************************************************/
|
|
|
|
#ifndef MATHEMATICS_H
|
|
#define MATHEMATICS_H
|
|
|
|
// Libraries
|
|
#include "Matrix.h"
|
|
#include "Matrix3x3.h"
|
|
#include "Quaternion.h"
|
|
#include "Vector.h"
|
|
#include "Vector3D.h"
|
|
#include "constants.h"
|
|
#include "exceptions.h"
|
|
#include "mathematics_functions.h"
|
|
#include <vector>
|
|
#include <cstdio>
|
|
#include <cassert>
|
|
#include <cmath>
|
|
|
|
// ReactPhysics3D namespace
|
|
namespace reactphysics3d {
|
|
|
|
// ---------- Mathematics functions ---------- //
|
|
|
|
// Rotate a vector according to a rotation quaternion.
|
|
// The function returns the vector rotated according to the quaternion in argument
|
|
inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d::Vector3D& vector, const reactphysics3d::Quaternion& quaternion) {
|
|
// Convert the vector into a quaternion
|
|
reactphysics3d::Quaternion vectorQuaternion(0, vector);
|
|
|
|
// Compute the quaternion rotation result
|
|
reactphysics3d::Quaternion quaternionResult = (quaternion * vectorQuaternion) * quaternion.getInverse();
|
|
|
|
// Convert the result quaternion into a vector
|
|
return quaternionResult.vectorV();
|
|
}
|
|
|
|
// Given two lines (given by the points "point1", "point2" and the vectors "d1" and "d2" that are not parallel, this method returns the values
|
|
// "alpha" and "beta" such that the two points P1 and P2 are the two closest point between the two lines and such that
|
|
// P1 = point1 + alpha * d1
|
|
// P2 = point2 + beta * d2
|
|
inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1, const reactphysics3d::Vector3D& d1, const reactphysics3d::Vector3D& point2,
|
|
const reactphysics3d::Vector3D& d2, double* alpha, double* beta) {
|
|
|
|
reactphysics3d::Vector3D r = point1 - point2;
|
|
double a = d1.scalarProduct(d1);
|
|
double b = d1.scalarProduct(d2);
|
|
double c = d1.scalarProduct(r);
|
|
double e = d2.scalarProduct(d2);
|
|
double f = d2.scalarProduct(r);
|
|
double d = a*e-b*b;
|
|
|
|
// The two lines must not be parallel
|
|
assert(!reactphysics3d::approxEqual(d, 0.0));
|
|
|
|
// Compute the "alpha" and "beta" values
|
|
*alpha = (b*f -c*e)/d;
|
|
*beta = (a*f-b*c)/d;
|
|
}
|
|
|
|
// This method returns true if the point "P" is on the segment between "segPointA" and "segPointB" and return false otherwise
|
|
inline bool isPointOnSegment(const reactphysics3d::Vector3D& segPointA, const reactphysics3d::Vector3D& segPointB, const reactphysics3d::Vector3D& P) {
|
|
|
|
// Check if the point P is on the line between "segPointA" and "segPointB"
|
|
reactphysics3d::Vector3D d = segPointB - segPointA;
|
|
reactphysics3d::Vector3D dP = P - segPointA;
|
|
if (!d.isParallelWith(dP)) {
|
|
return false;
|
|
}
|
|
|
|
// Compute the length of the segment
|
|
double segmentLength = d.length();
|
|
|
|
// Compute the distance from point "P" to points "segPointA" and "segPointB"
|
|
double distA = dP.length();
|
|
double distB = (P - segPointB).length();
|
|
|
|
// If one of the "distA" and "distB" is greather than the length of the segment, then P is not on the segment
|
|
if (distA > segmentLength || distB > segmentLength) {
|
|
return false;
|
|
}
|
|
|
|
// Otherwise, the point P is on the segment
|
|
return true;
|
|
}
|
|
|
|
|
|
// Given two lines in 3D that intersect, this method returns the intersection point between the two lines.
|
|
// The first line is given by the point "p1" and the vector "d1", the second line is given by the point "p2" and the vector "d2".
|
|
inline reactphysics3d::Vector3D computeLinesIntersection(const reactphysics3d::Vector3D& p1, const reactphysics3d::Vector3D& d1,
|
|
const reactphysics3d::Vector3D& p2, const reactphysics3d::Vector3D& d2) {
|
|
// Computes the two closest points on the lines
|
|
double alpha, beta;
|
|
closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta);
|
|
reactphysics3d::Vector3D point1 = p1 + alpha * d1;
|
|
reactphysics3d::Vector3D point2 = p2 + beta * d2;
|
|
|
|
// The two points must be very close
|
|
//assert((point1-point2).length() <= 0.1);
|
|
|
|
// Return the intersection point (halfway between "point1" and "point2")
|
|
return 0.5 * (point1 + point2);
|
|
}
|
|
|
|
|
|
// Given two segments in 3D that are not parallel and that intersect, this method computes the intersection point between the two segments.
|
|
// This method returns the intersection point.
|
|
inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB,
|
|
const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB) {
|
|
// Determine the lines of both segments
|
|
reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA;
|
|
reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA;
|
|
|
|
// The segments must not be parallel
|
|
assert(!d1.isParallelWith(d2));
|
|
|
|
// Compute the closet points between the two lines
|
|
double alpha, beta;
|
|
closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta);
|
|
reactphysics3d::Vector3D point1 = seg1PointA + alpha * d1;
|
|
reactphysics3d::Vector3D point2 = seg2PointA + beta * d2;
|
|
|
|
// The closest points have to be on the segments, otherwise there is no intersection between the segments
|
|
assert(isPointOnSegment(seg1PointA, seg1PointB, point1));
|
|
assert(isPointOnSegment(seg2PointA, seg2PointB, point2));
|
|
|
|
// If the two closest point aren't very close, there is no intersection between the segments
|
|
reactphysics3d::Vector3D d = point2 - point1;
|
|
assert(d.length() <= EPSILON);
|
|
|
|
// They are very close so we return the intersection point (halfway between "point1" and "point2"
|
|
return 0.5 * (point1 + point2);
|
|
}
|
|
|
|
|
|
// Move a set of points by a given vector.
|
|
// The method returns a set of points moved by the given vector.
|
|
inline std::vector<reactphysics3d::Vector3D> movePoints(const std::vector<reactphysics3d::Vector3D>& points, const reactphysics3d::Vector3D& vector) {
|
|
std::vector<reactphysics3d::Vector3D> result;
|
|
|
|
// For each point of the set
|
|
for (unsigned int i=0; i<points.size(); ++i) {
|
|
// Move the point
|
|
result.push_back(points[i] + vector);
|
|
}
|
|
|
|
// Return the result set of points
|
|
return result;
|
|
}
|
|
|
|
|
|
// Compute the projection of a set of 3D points onto a 3D plane. The set of points is given by "points" and the plane is given by
|
|
// a point "A" and a normal vector "normal". This method returns the initial set of points projected onto the plane.
|
|
inline std::vector<reactphysics3d::Vector3D> projectPointsOntoPlane(const std::vector<reactphysics3d::Vector3D>& points, const reactphysics3d::Vector3D& A,
|
|
const reactphysics3d::Vector3D& normal) {
|
|
assert(normal.length() != 0.0);
|
|
|
|
std::vector<Vector3D> projectedPoints;
|
|
reactphysics3d::Vector3D n = normal.getUnit();
|
|
|
|
// For each point of the set
|
|
for (unsigned int i=0; i<points.size(); ++i) {
|
|
// Compute the projection of the point onto the plane
|
|
projectedPoints.push_back(points[i] - (n * (points[i] - A).scalarProduct(n)));
|
|
|
|
}
|
|
|
|
// Return the projected set of points
|
|
return projectedPoints;
|
|
}
|
|
|
|
|
|
// Compute the distance between a point "P" and a line (given by a point "A" and a vector "v")
|
|
inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) {
|
|
assert(v.length() != 0);
|
|
return ((P-A).crossProduct(v).length() / (v.length()));
|
|
}
|
|
|
|
|
|
// Compute the orthogonal projection of a point "P" on a line (given by a point "A" and a vector "v")
|
|
inline reactphysics3d::Vector3D computeOrthogonalProjectionOfPointOntoALine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) {
|
|
return (A + ((P-A).scalarProduct(v) / (v.scalarProduct(v))) * v);
|
|
}
|
|
|
|
// Given a point P and 4 points that form a rectangle (point P and the 4 points have to be on the same plane) this method computes
|
|
// the point Q that is the nearest point to P that is inside (on a border of) the rectangle. The point P should be outside the rectangle.
|
|
// The result point Q will be in a segment of the rectangle
|
|
inline reactphysics3d::Vector3D computeNearestPointOnRectangle(const reactphysics3d::Vector3D& P, const std::vector<reactphysics3d::Vector3D> rectangle) {
|
|
assert(rectangle.size() == 4);
|
|
double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]);
|
|
double distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]);
|
|
double distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]);
|
|
double distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]);
|
|
double distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]);
|
|
double distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]);
|
|
Vector3D resultPoint;
|
|
|
|
// Check if P is between the lines of the first pair of parallel segments of the rectangle
|
|
if (distPSegment1 <= distSegment1Segment3 && distPSegment3 <= distSegment1Segment3) {
|
|
// Find among segments 2 and 4 which one is the nearest
|
|
if (distPSegment2 <= distPSegment4) { // Segment 2 is the nearest
|
|
// We compute the projection of the point P onto the segment 2
|
|
resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[1], rectangle[2] - rectangle[1]);
|
|
}
|
|
else { // Segment 4 is the nearest
|
|
// We compute the projection of the point P onto the segment 4
|
|
resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[3], rectangle[0] - rectangle[3]);
|
|
}
|
|
}
|
|
// Check if P is between the lines of the second pair of parallel segments of the rectangle
|
|
else if (distPSegment2 <= distSegment2Segment4 && distPSegment4 <= distSegment2Segment4) {
|
|
// Find among segments 1 and 3 which one is the nearest
|
|
if (distPSegment1 <= distPSegment3) { // Segment 1 is the nearest
|
|
// We compute the projection of the point P onto the segment 1
|
|
resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[0], rectangle[1] - rectangle[0]);
|
|
}
|
|
else { // Segment 3 is the nearest
|
|
// We compute the projection of the point P onto the segment 3
|
|
resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[2], rectangle[3] - rectangle[2]);
|
|
}
|
|
}
|
|
else if (distPSegment4 <= distPSegment2) {
|
|
if (distPSegment1 <= distPSegment3) { // The point P is in the corner of point rectangle[0]
|
|
// Return the corner of the rectangle
|
|
return rectangle[0];
|
|
}
|
|
else { // The point P is in the corner of point rectangle[3]
|
|
// Return the corner of the rectangle
|
|
return rectangle[3];
|
|
}
|
|
}
|
|
else {
|
|
if (distPSegment1 <= distPSegment3) { // The point P is in the corner of point rectangle[1]
|
|
// Return the corner of the rectangle
|
|
return rectangle[1];
|
|
}
|
|
else { // The point P is in the corner of point rectangle[2]
|
|
// Return the corner of the rectangle
|
|
return rectangle[2];
|
|
}
|
|
}
|
|
|
|
// Return the result point
|
|
return resultPoint;
|
|
}
|
|
|
|
// Compute the intersection between two parallel segments (the first segment is between the points "seg1PointA" and "seg1PointB" and the second
|
|
// segment is between the points "seg2PointA" and "seg2PointB"). The result is the segment intersection (represented by the points "resultPointA"
|
|
// and "resultPointB". Because the two given segments don't have to be on the same exact line, the result intersection segment will a segment
|
|
// halway between the first and the second given segments.
|
|
inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB,
|
|
const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB,
|
|
reactphysics3d::Vector3D& resultPointA, reactphysics3d::Vector3D& resultPointB) {
|
|
// Compute the segment vectors
|
|
reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA;
|
|
reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA;
|
|
|
|
// The two segments should be parallel
|
|
assert(d1.isParallelWith(d2));
|
|
|
|
// Compute the projection of the two points of the second segment onto the vector of segment 1
|
|
double projSeg2PointA = d1.getUnit().scalarProduct(seg2PointA - seg1PointA);
|
|
double projSeg2PointB = d1.getUnit().scalarProduct(seg2PointB - seg1PointA);
|
|
|
|
// The projections intervals should intersect
|
|
assert(!(projSeg2PointA < 0.0 && projSeg2PointB < 0.0));
|
|
assert(!(projSeg2PointA > d1.length() && projSeg2PointB > d1.length()));
|
|
|
|
// Compute the vector "v" from a point on the line 1 to the orthogonal point of the line 2
|
|
reactphysics3d::Vector3D point = computeOrthogonalProjectionOfPointOntoALine(seg2PointA, seg1PointA, d1);
|
|
reactphysics3d::Vector3D v = seg2PointA - point;
|
|
|
|
// Return the segment intersection according to the configuration of two projection intervals
|
|
if (projSeg2PointA >= 0 && projSeg2PointA <= d1.length() && projSeg2PointB >= d1.length()) {
|
|
// Move the contact points halfway between the two segments
|
|
resultPointA = seg2PointA - 0.5 * v;
|
|
resultPointB = seg1PointB + 0.5 * v;
|
|
}
|
|
else if (projSeg2PointA <= 0 && projSeg2PointB >= 0 && projSeg2PointB <= d1.length()) {
|
|
// Move the contact points halfway between the two segments
|
|
resultPointA = seg1PointA + 0.5 * v;
|
|
resultPointB = seg2PointB - 0.5 * v;
|
|
}
|
|
else if (projSeg2PointA <= 0 && projSeg2PointB >= d1.length()) {
|
|
// Move the contact points halfway between the two segments
|
|
resultPointA = seg1PointA + 0.5 * v;
|
|
resultPointB = seg1PointB + 0.5 * v;
|
|
}
|
|
else if (projSeg2PointA <= d1.length() && projSeg2PointB <= d1.length()) {
|
|
// Move the contact points halfway between the two segments
|
|
resultPointA = seg2PointA - 0.5 * v;
|
|
resultPointB = seg2PointB - 0.5 * v;
|
|
}
|
|
}
|
|
|
|
// This method clip a 3D segment with 3D rectangle polygon. The segment and the rectangle are asssumed to be on the same plane. We
|
|
// also assume that the segment is not completely outside the clipping rectangle.
|
|
// The segment is given by the two vertices in "segment" and the rectangle is given by the ordered vertices in "clipRectangle".
|
|
// This method returns the clipped segment.
|
|
inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(const std::vector<reactphysics3d::Vector3D>& segment, const std::vector<reactphysics3d::Vector3D> clipRectangle) {
|
|
double const epsilon = 0.01;
|
|
|
|
assert(segment.size() == 2);
|
|
assert(clipRectangle.size() == 4);
|
|
|
|
std::vector<reactphysics3d::Vector3D> inputSegment = segment;
|
|
std::vector<reactphysics3d::Vector3D> outputSegment;
|
|
|
|
// For each edge of the clip rectangle
|
|
for (unsigned int i=0; i<4; ++i) {
|
|
outputSegment.clear();
|
|
|
|
// Current clipped segment
|
|
//assert(inputSegment.size() == 2);
|
|
reactphysics3d::Vector3D S = inputSegment[0];
|
|
reactphysics3d::Vector3D P = inputSegment[1];
|
|
|
|
// Edge of the clip rectangle
|
|
reactphysics3d::Vector3D A = clipRectangle[i];
|
|
reactphysics3d::Vector3D B = clipRectangle[ (i+1) % 4];
|
|
reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
|
|
|
|
// If the point P is inside the clip plane
|
|
if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) {
|
|
// If the point S is inside the clip plane
|
|
if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) {
|
|
outputSegment.push_back(P);
|
|
outputSegment.push_back(S);
|
|
}
|
|
else { // P is inside and S is outside the clip plane
|
|
// Compute the intersection point between the segment SP and the clip plane
|
|
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
|
|
|
|
outputSegment.push_back(P);
|
|
outputSegment.push_back(intersectPoint);
|
|
}
|
|
}
|
|
else if (planeNormal.scalarProduct(S-A) > 0.0 - epsilon) { // P is outside and S is inside the clip plane
|
|
// Compute the intersection point between the segment SP and the clip plane
|
|
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
|
|
|
|
outputSegment.push_back(S);
|
|
outputSegment.push_back(intersectPoint);
|
|
}
|
|
|
|
inputSegment = outputSegment;
|
|
}
|
|
|
|
// Return the clipped segment
|
|
return outputSegment;
|
|
}
|
|
|
|
// This method uses the Sutherland-Hodgman clipping algorithm to clip a subject polygon (given by the ordered 3D vertices in "subjectPolygon") using
|
|
// a rectangle polygon (given by the ordered 3D vertices in "clipRectangle"). The subject polygon and the clip rectangle are in 3D but we assumed that
|
|
// they are on a same plane in 3D. The method returns the ordered 3D vertices of the subject polygon clipped using the rectangle polygon.
|
|
inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(const std::vector<reactphysics3d::Vector3D>& subjectPolygon, const std::vector<reactphysics3d::Vector3D>& clipRectangle) {
|
|
double const epsilon = 0.1;
|
|
assert(clipRectangle.size() == 4);
|
|
|
|
std::vector<reactphysics3d::Vector3D> outputPolygon;
|
|
std::vector<reactphysics3d::Vector3D> inputPolygon = subjectPolygon;
|
|
|
|
// For each edge of the clip rectangle
|
|
for (unsigned int i=0; i<4; ++i) {
|
|
outputPolygon.clear();
|
|
|
|
// Each edge defines a clip plane. The clip plane is define by a point on this plane (a vertice of the current edge) and
|
|
// a plane normal (because we are using a clip rectangle, the plane normal is the next edge of the clip rectangle).
|
|
reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
|
|
reactphysics3d::Vector3D A = clipRectangle[i]; // Segment AB is the current segment of the "clipRectangle"
|
|
reactphysics3d::Vector3D B = clipRectangle[(i+1) % 4];
|
|
reactphysics3d::Vector3D S = inputPolygon[0];
|
|
|
|
// For each vertex of the subject polygon
|
|
for (unsigned int j=0; j<inputPolygon.size(); ++j) {
|
|
reactphysics3d::Vector3D P = inputPolygon[(j+1) % inputPolygon.size()];
|
|
|
|
// If the point P is inside the clip plane
|
|
double test = planeNormal.scalarProduct(P-A);
|
|
if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) {
|
|
// If the point S is also inside the clip plane
|
|
if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) {
|
|
outputPolygon.push_back(P);
|
|
}
|
|
else { // If the point S is outside the clip plane
|
|
// Compute the intersection point between the segment SP and the clip plane
|
|
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
|
|
|
|
outputPolygon.push_back(intersectPoint);
|
|
outputPolygon.push_back(P);
|
|
}
|
|
}
|
|
else if (planeNormal.scalarProduct(S-A) > 0.0) {
|
|
// Compute the intersection point between the segment SP and the clip plane
|
|
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
|
|
|
|
outputPolygon.push_back(intersectPoint);
|
|
}
|
|
S = P;
|
|
}
|
|
inputPolygon = outputPolygon;
|
|
}
|
|
|
|
// Return the clipped polygon
|
|
return outputPolygon;
|
|
}
|
|
|
|
// Compute the intersection point between a line and a plane in 3D space. There must be an intersection, therefore the
|
|
// the lineVector must not be orthogonal to the planeNormal.
|
|
inline reactphysics3d::Vector3D intersectLineWithPlane(const reactphysics3d::Vector3D& linePoint, const reactphysics3d::Vector3D& lineVector,
|
|
const reactphysics3d::Vector3D& planePoint, const reactphysics3d::Vector3D& planeNormal) {
|
|
assert(!approxEqual(lineVector.scalarProduct(planeNormal), 0.0));
|
|
|
|
// The plane is represented by the equation planeNormal dot X = d where X is a point of the plane
|
|
double d = planeNormal.scalarProduct(planePoint);
|
|
|
|
// Compute the parameter t
|
|
double t = (d - planeNormal.scalarProduct(linePoint)) / planeNormal.scalarProduct(lineVector);
|
|
|
|
// Compute the intersection point
|
|
return linePoint + lineVector * t;
|
|
}
|
|
|
|
|
|
} // End of the ReactPhysics3D namespace
|
|
|
|
|
|
#endif
|