3dpcp/.svn/pristine/bc/bc45eefcadfc139469d17a510dd463a1ddead50d.svn-base

146 lines
2.8 KiB
Text
Raw Normal View History

2012-09-16 12:33:11 +00:00
/*
* geom_math implementation
*
* Copyright (C) Dorit Borrmann
*
* Released under the GPL version 3.
*
*/
#include "math.h"
#include "shapes/geom_math.h"
float Nx,Ny,Nz,Nd;
float Cx, Cy, Cz;
float maxDist = 1.0; // cm
float maxRadius = 105.0; // cm
float CxM, CxP, CyM, CyP, CzM, CzP;
bool SphereInAABB( float x, float y, float z, float size ) {
if ( x + size < CxM ||
x - size > CxP ||
y + size < CyM ||
y - size > CyP ||
z + size < CzM ||
z - size > CzP ) {
return false;
}
return true;
}
void setNumber(double *plane, double *center, double _radius, double _maxDist) {
maxDist = _maxDist;
maxRadius = _radius;
Nx = plane[0];
Ny = plane[1];
Nz = plane[2];
Nd = plane[3];
Cx = center[0];
Cy = center[1];
Cz = center[2];
CxM = Cx - maxRadius;
CxP = Cx + maxRadius;
CyM = Cy - maxRadius;
CyP = Cy + maxRadius;
CzM = Cz - maxRadius;
CzP = Cz + maxRadius;
}
bool PlaneInCube( float x, float y, float z, float size, float nx, float ny, float nz, float d)
{
float xm, xp, ym, yp, zm, zp;
float Fxm, Fxp, Fym, Fyp, Fzm, Fzp;
xm = x - size;
xp = x + size;
ym = y - size;
yp = y + size;
zm = z - size;
zp = z + size;
Fxm = nx * xm;
Fym = ny * ym;
Fzm = nz * zm;
bool positive = (Fxm + Fym + Fzm + d > 0);
Fxp = nx * xp;
if( (Fxp + Fym + Fzm + d < 0) == positive)
return true;
Fyp = ny * yp;
if( (Fxm + Fyp + Fzm + d < 0) == positive )
return true;
if( (Fxp + Fyp + Fzm + d < 0) == positive )
return true;
Fzp = nz * zp;
if( (Fxm + Fym + Fzp + d < 0) == positive )
return true;
if( (Fxp + Fym + Fzp + d < 0) == positive )
return true;
if( (Fxm + Fyp + Fzp + d < 0) == positive )
return true;
if( (Fxp + Fyp + Fzp + d < 0) == positive )
return true;
return false;
}
bool PlaneInCube( float x, float y, float z, float size)
{
float xm, xp, ym, yp, zm, zp;
float Fxm, Fxp, Fym, Fyp, Fzm, Fzp;
xm = x - size;
xp = x + size;
ym = y - size;
yp = y + size;
zm = z - size;
zp = z + size;
Fxm = Nx * xm;
Fym = Ny * ym;
Fzm = Nz * zm;
bool positive = (Fxm + Fym + Fzm + Nd > 0);
Fxp = Nx * xp;
if( (Fxp + Fym + Fzm + Nd < 0) == positive)
return true;
Fyp = Ny * yp;
if( (Fxm + Fyp + Fzm + Nd < 0) == positive )
return true;
if( (Fxp + Fyp + Fzm + Nd < 0) == positive )
return true;
Fzp = Nz * zp;
if( (Fxm + Fym + Fzp + Nd < 0) == positive )
return true;
if( (Fxp + Fym + Fzp + Nd < 0) == positive )
return true;
if( (Fxm + Fyp + Fzp + Nd < 0) == positive )
return true;
if( (Fxp + Fyp + Fzp + Nd < 0) == positive )
return true;
return false;
}
bool closeToPlane(double *p) {
return ( fabs(planeDist(p, Nx, Ny, Nz, Nd)) < maxDist );
}