void Actor::updateWalk() {
if (_path.empty()) {
return;
}
Math::Vector3d destPos = _path.back();
Math::Vector3d dir = destPos - _pos;
float dist = dir.getMagnitude();
_walkedCur = true;
float walkAmt = g_grim->getPerSecond(_walkRate);
if (walkAmt >= dist) {
_path.pop_back();
if (_path.empty()) {
_walking = false;
_pos = destPos;
// It seems that we need to allow an already active turning motion to
// continue or else turning actors away from barriers won't work right
_turning = false;
return;
}
}
destPos = handleCollisionTo(_pos, destPos);
Math::Angle y = getYawTo(destPos);
turnTo(_pitch, y, _roll);
dir = destPos - _pos;
dir.normalize();
_pos += dir * walkAmt;
}
void Lua_V2::WorldToScreen() {
lua_Object xObj = lua_getparam(1);
lua_Object yObj = lua_getparam(2);
lua_Object zObj = lua_getparam(3);
if (!lua_isnumber(xObj) || !lua_isnumber(yObj) || !lua_isnumber(zObj)) {
lua_pushnumber(0.0);
lua_pushnumber(0.0);
return;
}
float x = lua_getnumber(xObj);
float y = lua_getnumber(yObj);
float z = lua_getnumber(zObj);
Math::Vector3d pos = Math::Vector3d(x, y, z);
const Set::Setup *setup = g_emi->getCurrSet()->getCurrSetup();
const Math::Vector3d interest = setup->_interest;
const float roll = setup->_roll;
const Math::Quaternion quat = Math::Quaternion(interest.x(), interest.y(), interest.z(), roll);
Math::Matrix4 view = quat.toMatrix();
view.transpose();
pos -= setup->_pos;
pos = view.getRotation() * pos;
pos.z() = -pos.z();
Math::Matrix4 proj = GfxBase::makeProjMatrix(setup->_fov, setup->_nclip, setup->_fclip);
proj.transpose();
Math::Vector4d screen = proj * Math::Vector4d(pos.x(), pos.y(), pos.z(), 1.0);
screen /= screen.w();
lua_pushnumber((screen.x() + 1) * 320);
lua_pushnumber((1 - screen.y()) * 240);
}
void Lua_V1::SetSoundPosition() {
Math::Vector3d pos;
int minVolume = 10;
int maxVolume = 127;
float someParam = 0;
int argId = 1;
lua_Object paramObj;
if (g_grim->getCurrSet()) {
g_grim->getCurrSet()->getSoundParameters(&minVolume, &maxVolume);
}
lua_Object nameObj = lua_getparam(argId++);
if (!lua_isnumber(nameObj) && !lua_isstring(nameObj))
return;
lua_Object actorObj = lua_getparam(argId++);
if (lua_isuserdata(actorObj) && lua_tag(actorObj) == MKTAG('A','C','T','R')) {
Actor *actor = getactor(actorObj);
if (!actor)
return;
pos = actor->getPos();
} else if (lua_isnumber(actorObj)) {
float x = lua_getnumber(actorObj);
float y = lua_getnumber(argId++);
float z = lua_getnumber(argId++);
pos.set(x, y, z);
}
paramObj = (int)lua_getparam(argId++);
if (lua_isnumber(paramObj)) {
minVolume = (int)lua_getnumber(paramObj);
if (minVolume > 127)
minVolume = 127;
}
paramObj = lua_getparam(argId++);
if (lua_isnumber(paramObj)) {
maxVolume = (int)lua_getnumber(paramObj);
if (maxVolume > 127)
maxVolume = 127;
else if (maxVolume < minVolume)
maxVolume = minVolume;
}
paramObj = lua_getparam(argId++);
if (lua_isnumber(paramObj)) {
someParam = (int)lua_getnumber(paramObj);
if (someParam < 0.0)
someParam = 0.0;
}
if (g_grim->getCurrSet()) {
if (lua_isnumber(nameObj))
error("SetSoundPosition: number is not yet supported");
else {
const char *soundName = lua_getstring(nameObj);
g_grim->getCurrSet()->setSoundPosition(soundName, pos, minVolume, maxVolume);
}
}
}
void SoundTrack::updatePosition() {
if (!_positioned)
return;
Set *set = g_grim->getCurrSet();
Set::Setup *setup = set->getCurrSetup();
Math::Vector3d cameraPos = setup->_pos;
Math::Vector3d vector = _pos - cameraPos;
float distance = vector.getMagnitude();
_attenuation = MAX(0.0f, 1.0f - distance / (_volume * 100.0f / Audio::Mixer::kMaxChannelVolume));
if (!isfinite(_attenuation)) {
_attenuation = 0.0f;
}
Math::Matrix4 worldRot = setup->_rot;
Math::Vector3d relPos = (_pos - setup->_pos);
Math::Vector3d p(relPos);
worldRot.inverseRotate(&p);
float angle = atan2(p.x(), p.z());
float pan = sin(angle);
_balance = (int)(pan * 127.0f);
if (_handle) {
g_system->getMixer()->setChannelBalance(*_handle, _balance);
g_system->getMixer()->setChannelVolume(*_handle, (byte)getEffectiveVolume());
}
}
void ShaderRenderer::screenPosToDirection(const Common::Point screen, float &pitch, float &heading) {
double x, y, z;
x = screen.x;
y = kOriginalHeight - screen.y;
z = 0.9f;
const Math::Vector2d tl = _viewport.getTopLeft();
x = 2 * double(x - tl.getX()) / _viewport.getWidth() - 1.0f;
y = 2 * double(y - tl.getY()) / _viewport.getHeight() - 1.0f;
z = 2 * z - 1.0f;
// Screen coords to 3D coords
Math::Vector4d point = Math::Vector4d(x, y, z, 1.0f);
point = _mvpMatrix * point;
// 3D coords to polar coords
Math::Vector3d v = Math::Vector3d(point.x(), point.y(), point.z());
v.normalize();
Math::Vector2d horizontalProjection = Math::Vector2d(v.x(), v.z());
horizontalProjection.normalize();
pitch = 90 - Math::Angle::arcCosine(v.y()).getDegrees();
heading = Math::Angle::arcCosine(horizontalProjection.getY()).getDegrees();
if (horizontalProjection.getX() > 0.0)
heading = 360 - heading;
}
/* Given a position and a size this function calculates and pushes
* the nearest point to that which will be valid if the boxes are
* shrunk by the amount specified.
*/
void Lua_V1::GetShrinkPos() {
lua_Object xObj = lua_getparam(1);
lua_Object yObj = lua_getparam(2);
lua_Object zObj = lua_getparam(3);
lua_Object rObj = lua_getparam(4);
if (!lua_isnumber(xObj) || !lua_isnumber(yObj) || !lua_isnumber(zObj) || !lua_isnumber(rObj))
return;
float x = lua_getnumber(xObj);
float y = lua_getnumber(yObj);
float z = lua_getnumber(zObj);
float r = lua_getnumber(rObj);
Math::Vector3d pos;
pos.set(x, y, z);
Sector* sector;
g_grim->getCurrSet()->shrinkBoxes(r);
g_grim->getCurrSet()->findClosestSector(pos, §or, &pos);
g_grim->getCurrSet()->unshrinkBoxes();
if (sector) {
lua_pushnumber(pos.x());
lua_pushnumber(pos.y());
lua_pushnumber(pos.z());
} else {
lua_pushnil();
}
}
void Lua_V2::WalkActorToAvoiding() {
lua_Object actorObj = lua_getparam(1);
lua_Object actor2Obj = lua_getparam(2);
lua_Object xObj = lua_getparam(3);
lua_Object yObj = lua_getparam(4);
lua_Object zObj = lua_getparam(5);
if (!lua_isuserdata(actorObj) || lua_tag(actorObj) != MKTAG('A','C','T','R'))
return;
if (!lua_isuserdata(actor2Obj) || lua_tag(actor2Obj) != MKTAG('A','C','T','R'))
return;
Math::Vector3d destVec;
Actor *actor = getactor(actorObj);
if (!lua_isnumber(xObj)) {
if (!lua_isuserdata(xObj) || lua_tag(xObj) != MKTAG('A','C','T','R'))
return;
Actor *destActor = getactor(xObj);
destVec = destActor->getPos();
} else {
float x = lua_getnumber(xObj);
float y = lua_getnumber(yObj);
float z = lua_getnumber(zObj);
destVec.set(x, y, z);
}
// TODO: Make this actually avoid the second actor
actor->walkTo(destVec);
}
void Lua_V2::GetActorPuckVector() {
lua_Object actorObj = lua_getparam(1);
lua_Object addObj = lua_getparam(2);
if (!lua_isuserdata(actorObj) || lua_tag(actorObj) != MKTAG('A','C','T','R')) {
lua_pushnil();
return;
}
Actor *actor = getactor(actorObj);
// Note: The wear chore of dumbshadow.cos is only started from Lua if
// GetActorPuckVector returns a non-nil value. The original engine seems
// to return nil for all actors that have never followed walkboxes.
if (!actor || !actor->hasFollowedBoxes()) {
lua_pushnil();
return;
}
Math::Vector3d result = actor->getPuckVector();
if (!lua_isnil(addObj))
result += actor->getPos();
lua_pushnumber(result.x());
lua_pushnumber(result.y());
lua_pushnumber(result.z());
}
// 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];
}
Math::Vector3d Sector::getProjectionToPuckVector(const Math::Vector3d &v) const {
if (_normal.z() == 0)
error("Trying to walk along vertical plane");
Math::Vector3d result = v;
result.z() -= Math::Vector3d::dotProduct(_normal, v) / _normal.z();
return result;
}
Command *Command::opIsItemNearPlace(const ResourceReference &itemRef, const ResourceReference &positionRef, int32 testDistance) {
FloorPositionedItem *item = itemRef.resolve<FloorPositionedItem>();
Math::Vector3d itemPostion = item->getPosition3D();
Math::Vector3d testPosition = getObjectPosition(positionRef);
return nextCommandIf(itemPostion.getDistanceTo(testPosition) < testDistance);
}
Math::Vector3d Sector::getProjectionToPlane(const Math::Vector3d &point) const {
if (_normal.z() == 0)
error("Trying to walk along vertical plane");
// Formula: return p - (n . (p - v_0))/(n . k) k
Math::Vector3d result = point;
result.z() -= Math::Vector3d::dotProduct(_normal, point - _vertices[0]) / _normal.z();
return result;
}
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.
// The plane has equation ax + by + cz + d = 0
float a = _normal.x();
float b = _normal.y();
float c = _normal.z();
float d = -_vertices[0].x() * a - _vertices[0].y() * b - _vertices[0].z() * c;
float dist = (a * point.x() + b * point.y() + c * point.z() + d) /
sqrt(a * a + b * b + c * c);
// dist is positive if it is above the plain, negative if it is
// below and 0 if it is on the plane.
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];
if (edge.x() * delta.y() < edge.y() * delta.x())
return false;
}
return true;
}
static Math::Vector3d directionToVector(float pitch, float heading) {
Math::Vector3d v;
float radHeading = Common::deg2rad(heading);
float radPitch = Common::deg2rad(pitch);
v.setValue(0, cos(radPitch) * cos(radHeading));
v.setValue(1, sin(radPitch));
v.setValue(2, cos(radPitch) * sin(radHeading));
return v;
}
Math::Angle Actor::getYawTo(Actor *a) const {
Math::Vector3d forwardVec = getSimplePuckVector();
Math::Vector3d delta = a->getPos() - _pos;
if (g_grim->getGameType() == GType_MONKEY4) {
delta.y() = 0;
} else {
delta.z() = 0;
}
return Math::Vector3d::angle(forwardVec, delta);
}
float Sector::distanceToPoint(const Math::Vector3d &point) const {
// The plane has equation ax + by + cz + d = 0
float a = _normal.x();
float b = _normal.y();
float c = _normal.z();
float d = -_vertices[0].x() * a - _vertices[0].y() * b - _vertices[0].z() * c;
// dist is positive if it is above the plain, negative if it is
// below and 0 if it is on the plane.
float dist = (a * point.x() + b * point.y() + c * point.z() + d);
dist /= sqrt(a * a + b * b + c * c);
return dist;
}
Math::Vector3d Actor::getTangentPos(const Math::Vector3d &pos, const Math::Vector3d &dest) const {
if (_collisionMode == CollisionOff) {
return dest;
}
Model *model = getCurrentCostume()->getModel();
Math::Vector3d p = _pos + model->_insertOffset;
float size = model->_radius * _collisionScale;
Math::Vector2d p1(pos.x(), pos.y());
Math::Vector2d p2(dest.x(), dest.y());
Math::Segment2d segment(p1, p2);
// TODO: collision with Box
// if (_collisionMode == CollisionSphere) {
Math::Vector2d center(p.x(), p.y());
Math::Vector2d inter;
float distance = segment.getLine().getDistanceTo(center, &inter);
if (distance < size && segment.containsPoint(inter)) {
Math::Vector2d v(inter - center);
v.normalize();
v *= size;
v += center;
return Math::Vector3d(v.getX(), v.getY(), dest.z());
}
// } else {
// }
return dest;
}
void Lua_V2::GetActorWorldPos() {
lua_Object actorObj = lua_getparam(1);
if (!lua_isuserdata(actorObj) || lua_tag(actorObj) != MKTAG('A','C','T','R'))
return;
Actor *actor = getactor(actorObj);
if (!actor)
return;
Math::Vector3d pos = actor->getWorldPos();
lua_pushnumber(pos.x());
lua_pushnumber(pos.y());
lua_pushnumber(pos.z());
}
float Movement::computeAngleBetweenVectorsXYPlane(const Math::Vector3d &v1, const Math::Vector3d &v2) const {
Math::Vector3d v1XY = v1;
v1XY.z() = 0.0;
Math::Vector3d v2XY = v2;
v2XY.z() = 0.0;
Math::Angle angle = Math::Vector3d::angle(v1XY, v2XY);
Math::Vector3d cross = Math::Vector3d::crossProduct(v1XY, v2XY);
if (cross.z() < 0) {
angle = -angle;
}
return angle.getDegrees();
}
void BaseRenderer::screenPosToDirection(const Common::Point screen, float &pitch, float &heading) {
// Screen coords to 3D coords
Math::Vector3d obj;
Math::gluMathUnProject(Math::Vector3d(screen.x, _system->getHeight() - screen.y, 0.9f), _mvpMatrix, frameViewport(), obj);
// 3D coords to polar coords
obj.normalize();
Math::Vector2d horizontalProjection = Math::Vector2d(obj.x(), obj.z());
horizontalProjection.normalize();
pitch = 90 - Math::Angle::arcCosine(obj.y()).getDegrees();
heading = Math::Angle::arcCosine(horizontalProjection.getY()).getDegrees();
if (horizontalProjection.getX() > 0.0)
heading = 360 - heading;
}
void TinyGLRenderer::screenPosToDirection(const Common::Point screen, float &pitch, float &heading) {
// Screen coords to 3D coords
Math::Vector3d obj;
Math::gluMathUnProject<float, int>(Math::Vector3d(screen.x, kOriginalHeight - screen.y, 0.9),
_cubeModelViewMatrix, _cubeProjectionMatrix, _cubeViewport, obj);
// 3D coords to polar coords
obj.normalize();
Math::Vector2d horizontalProjection = Math::Vector2d(obj.x(), obj.z());
horizontalProjection.normalize();
pitch = 90 - Math::Angle::arcCosine(obj.y()).getDegrees();
heading = Math::Angle::arcCosine(horizontalProjection.getY()).getDegrees();
if (horizontalProjection.getX() > 0.0)
heading = 360 - heading;
}
void Renderer::screenPosToDirection(const Common::Point screen, float &pitch, float &heading) {
double x, y, z;
// Screen coords to 3D coords
gluUnProject(screen.x, kOriginalHeight - screen.y, 0.9, _cubeModelViewMatrix, _cubeProjectionMatrix, (GLint *)_cubeViewport, &x, &y, &z);
// 3D coords to polar coords
Math::Vector3d v = Math::Vector3d(x, y, z);
v.normalize();
Math::Vector2d horizontalProjection = Math::Vector2d(v.x(), v.z());
horizontalProjection.normalize();
pitch = 90 - Math::Angle::arcCosine(v.y()).getDegrees();
heading = Math::Angle::arcCosine(horizontalProjection.getY()).getDegrees();
if (horizontalProjection.getX() > 0.0)
heading = 360 - heading;
}
void Lua_V1::GetSectorOppositeEdge() {
lua_Object actorObj = lua_getparam(1);
lua_Object nameObj = lua_getparam(2);
if (!lua_isuserdata(actorObj) || lua_tag(actorObj) != MKTAG('A','C','T','R'))
return;
if (!lua_isstring(nameObj)) {
lua_pushnil();
return;
}
Actor *actor = getactor(actorObj);
const char *name = lua_getstring(nameObj);
int numSectors = g_grim->getCurrSet()->getSectorCount();
for (int i = 0; i < numSectors; i++) {
Sector *sector = g_grim->getCurrSet()->getSectorBase(i);
if (strmatch(sector->getName(), name)) {
if (sector->getNumVertices() != 4)
warning("GetSectorOppositeEdge(): cheat box with %d (!= 4) edges!", sector->getNumVertices());
Math::Vector3d* vertices = sector->getVertices();
Sector::ExitInfo e;
sector->getExitInfo(actor->getPos(), -actor->getPuckVector(), &e);
float frac = (e.exitPoint - vertices[e.edgeVertex + 1]).getMagnitude() / e.edgeDir.getMagnitude();
e.edgeVertex -= 2;
if (e.edgeVertex < 0)
e.edgeVertex += sector->getNumVertices();
Math::Vector3d edge = vertices[e.edgeVertex + 1] - vertices[e.edgeVertex];
Math::Vector3d p = vertices[e.edgeVertex] + edge * frac;
lua_pushnumber(p.x());
lua_pushnumber(p.y());
lua_pushnumber(p.z());
return;
}
}
lua_pushnil();
}
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;
}
void Set::setSoundPosition(const char *soundName, const Math::Vector3d &pos, int minVol, int maxVol) {
// TODO: The volume and pan needs to be updated when the setup changes.
Math::Vector3d cameraPos = _currSetup->_pos;
Math::Vector3d vector = pos - cameraPos;
float distance = vector.getMagnitude();
float diffVolume = maxVol - minVol;
//This 8.f is a guess, so it may need some adjusting
int newVolume = (int)(8.f * diffVolume / distance);
newVolume += minVol;
if (newVolume > _maxVolume)
newVolume = _maxVolume;
g_sound->setVolume(soundName, newVolume);
Math::Vector3d cameraVector =_currSetup->_interest - _currSetup->_pos;
Math::Vector3d up(0,0,1);
Math::Vector3d right;
cameraVector.normalize();
float roll = -_currSetup->_roll * LOCAL_PI / 180.f;
float cosr = cos(roll);
// Rotate the up vector by roll.
up = up * cosr + Math::Vector3d::crossProduct(cameraVector, up) * sin(roll) +
cameraVector * Math::Vector3d::dotProduct(cameraVector, up) * (1 - cosr);
right = Math::Vector3d::crossProduct(cameraVector, up);
right.normalize();
float angle = atan2(Math::Vector3d::dotProduct(vector, right),
Math::Vector3d::dotProduct(vector, cameraVector));
float pan = sin(angle);
g_sound->setPan(soundName, (int)((pan + 1.f) / 2.f * 127.f + 0.5f));
}
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;
}
Math::Angle Actor::getYawTo(const Math::Vector3d &p) const {
Math::Vector3d dpos = p - _pos;
if (g_grim->getGameType() == GType_MONKEY4) {
dpos.y() = dpos.z();
}
if (dpos.x() == 0 && dpos.y() == 0)
return 0;
else
return Math::Angle::arcTangent2(-dpos.x(), dpos.y());
}
void Lua_V2::GetActorPuckVector() {
lua_Object actorObj = lua_getparam(1);
lua_Object addObj = lua_getparam(2);
if (!lua_isuserdata(actorObj) || lua_tag(actorObj) != MKTAG('A','C','T','R')) {
lua_pushnil();
return;
}
Actor *actor = getactor(actorObj);
if (!actor) {
lua_pushnil();
return;
}
Math::Vector3d result = actor->getPuckVector();
if (!lua_isnil(addObj))
result += actor->getPos();
lua_pushnumber(result.x());
lua_pushnumber(result.y());
lua_pushnumber(result.z());
}
bool FloorFace::isPointInside(const Math::Vector3d &point) const {
// Compute the barycentric coordinates of the point in the triangle
float area = 1.0 / 2.0
* (-_vertices[1].y() * _vertices[2].x()
+ _vertices[0].y() * (-_vertices[1].x() + _vertices[2].x())
+ _vertices[0].x() * (_vertices[1].y() - _vertices[2].y())
+ _vertices[1].x() * _vertices[2].y());
int32 sign = area < 0 ? -1 : 1;
float s = (_vertices[0].y() * _vertices[2].x() - _vertices[0].x() * _vertices[2].y()
+ (_vertices[2].y() - _vertices[0].y()) * point.x()
+ (_vertices[0].x() - _vertices[2].x()) * point.y())
* sign;
float t = (_vertices[0].x() * _vertices[1].y() - _vertices[0].y() * _vertices[1].x()
+ (_vertices[0].y() - _vertices[1].y()) * point.x()
+ (_vertices[1].x() - _vertices[0].x()) * point.y())
* sign;
// Check the coordinates are in the triangle
return s > 0 && t > 0 && (s + t) < 2.0 * area * sign;
}
void FloorFace::computePointHeight(Math::Vector3d &point) const {
// Compute the barycentric coordinates of the point in the triangle
float area = 1.0 / 2.0
* (-_vertices[1].y() * _vertices[2].x()
+ _vertices[0].y() * (-_vertices[1].x() + _vertices[2].x())
+ _vertices[0].x() * (_vertices[1].y() - _vertices[2].y())
+ _vertices[1].x() * _vertices[2].y());
float s = (_vertices[0].y() * _vertices[2].x() - _vertices[0].x() * _vertices[2].y()
+ (_vertices[2].y() - _vertices[0].y()) * point.x()
+ (_vertices[0].x() - _vertices[2].x()) * point.y())
/ (2.0 * area);
float t = (_vertices[0].x() * _vertices[1].y() - _vertices[0].y() * _vertices[1].x()
+ (_vertices[0].y() - _vertices[1].y()) * point.x()
+ (_vertices[1].x() - _vertices[0].x()) * point.y())
/ (2.0 * area);
// Compute the Z coordinate of the point
float pointZ = (1.0 - s - t) * _vertices[0].z() + s * _vertices[1].z() + t * _vertices[2].z();
point.setValue(2, pointZ);
}