static void build_m3( GLfloat f[][3], GLfloat m[],
const GLvector4f *normal,
const GLvector4f *eye )
{
GLuint stride = eye->stride;
GLfloat *coord = (GLfloat *)eye->start;
GLuint count = eye->count;
const GLfloat *norm = normal->start;
GLuint i;
for (i=0;i<count;i++,STRIDE_F(coord,stride),STRIDE_F(norm,normal->stride)) {
GLfloat u[3], two_nu, fx, fy, fz;
COPY_3V( u, coord );
NORMALIZE_3FV( u );
two_nu = 2.0F * DOT3(norm,u);
fx = f[i][0] = u[0] - norm[0] * two_nu;
fy = f[i][1] = u[1] - norm[1] * two_nu;
fz = f[i][2] = u[2] - norm[2] * two_nu;
m[i] = fx * fx + fy * fy + (fz + 1.0F) * (fz + 1.0F);
if (m[i] != 0.0F) {
m[i] = 0.5F * (1.0f / sqrtf(m[i]));
}
}
}
int AseFile::ReadMesh_Normals(zASE_Object &obj)
{
char temp[255];
int index=0,i;
if( obj.numOfVerts!=0)obj.pNormals = new vec[obj.numOfVerts];
else return ReadUnknown( temp);
if( obj.numOfFaces!=0)obj.pFaceNormals = new vec[obj.numOfFaces];
do
{
if( !fgets(temp, 255, file) )return 0;
// *MESH_FACENORMAL 0 0.00000 0.00000 -1.00000
// *MESH_VERTEXNORMAL 0 0.00000 0.00000 -1.00000
// *MESH_VERTEXNORMAL 2 0.00000 0.00000 -1.00000
// *MESH_VERTEXNORMAL 3 0.00000 0.00000 -1.00000
if( EqualString( temp, "*MESH_FACENORMAL ") )
{
sscanf( temp, "%d", &index);
if(index>=obj.numOfFaces || index<0)continue;
// sscanf( temp, "%d %f %f %f", &i, &obj.pFaceNormals[index].x, &obj.pFaceNormals[index].y, &obj.pFaceNormals[index].z );
vec in; sscanf( temp, "%d %f %f %f", &i, &in.x, &in.y, &in.z );
obj.pFaceNormals[index].x = DOT3( obj.rotmatrix[0], in );
obj.pFaceNormals[index].y = DOT3( obj.rotmatrix[1], in );
obj.pFaceNormals[index].z = DOT3( obj.rotmatrix[2], in );
obj.pFaceNormals[index].Normalize();
}
else if (EqualString( temp, "*MESH_VERTEXNORMAL "))
{
sscanf( temp, "%d", &index);
if(index>=obj.numOfVerts || index<0)continue;
// sscanf( temp, "%d %f %f %f", &i, &obj.pNormals[index].x, &obj.pNormals[index].y, &obj.pNormals[index].z );
vec in; sscanf( temp, "%d %f %f %f", &i, &in.x, &in.y, &in.z );
obj.pNormals[index].x = DOT3( obj.rotmatrix[0], in );
obj.pNormals[index].y = DOT3( obj.rotmatrix[1], in );
obj.pNormals[index].z = DOT3( obj.rotmatrix[2], in );
// obj.pNormals[index].Normalize();
}
// ReadUnknown( temp);
}while(!FindBracketClose( temp)); // while temp not contain '}'
return 1;
}
/** The following code is from Christer Ericson's book Real-Time Collision Detection, pp. 101-106.
http://realtimecollisiondetection.net/ */
bool OBB::Intersects(const OBB &b, float epsilon) const
{
assume(pos.IsFinite());
assume(b.pos.IsFinite());
assume(float3::AreOrthonormal(axis[0], axis[1], axis[2]));
assume(float3::AreOrthonormal(b.axis[0], b.axis[1], b.axis[2]));
// Generate a rotation matrix that transforms from world space to this OBB's coordinate space.
float3x3 R;
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
R[i][j] = Dot(axis[i], b.axis[j]);
float3 t = b.pos - pos;
// Express the translation vector in a's coordinate frame.
t = float3(Dot(t, axis[0]), Dot(t, axis[1]), Dot(t, axis[2]));
float3x3 AbsR;
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
AbsR[i][j] = Abs(R[i][j]) + epsilon;
// Test the three major axes of this OBB.
for(int i = 0; i < 3; ++i)
{
float ra = r[i];
float rb = DOT3(b.r, AbsR[i]);
if (Abs(t[i]) > ra + rb)
return false;
}
// Test the three major axes of the OBB b.
for(int i = 0; i < 3; ++i)
{
float ra = r[0] * AbsR[0][i] + r[1] * AbsR[1][i] + r[2] * AbsR[2][i];
float rb = b.r[i];
if (Abs(t.x + R[0][i] + t.y * R[1][i] + t.z * R[2][i]) > ra + rb)
return false;
}
// Test the 9 different cross-axes.
// A.x <cross> B.x
float ra = r.y * AbsR[2][0] + r.z * AbsR[1][0];
float rb = b.r.y * AbsR[0][2] + b.r.z * AbsR[0][1];
if (Abs(t.z * R[1][0] - t.y * R[2][0]) > ra + rb)
return false;
// A.x < cross> B.y
ra = r.y * AbsR[2][1] + r.z * AbsR[1][1];
rb = b.r.x * AbsR[0][2] + b.r.z * AbsR[0][0];
if (Abs(t.z * R[1][1] - t.y * R[2][1]) > ra + rb)
return false;
// A.x <cross> B.z
ra = r.y * AbsR[2][2] + r.z * AbsR[1][2];
rb = b.r.x * AbsR[0][1] + b.r.y * AbsR[0][0];
if (Abs(t.z * R[1][22] - t.y * R[2][2]) > ra + rb)
return false;
// A.y <cross> B.x
ra = r.x * AbsR[2][0] + r.z * AbsR[0][0];
rb = b.r.y * AbsR[1][2] + b.r.z * AbsR[1][1];
if (Abs(t.x * R[2][0] - t.z * R[0][0]) > ra + rb)
return false;
// A.y <cross> B.y
ra = r.x * AbsR[2][1] + r.z * AbsR[0][1];
rb = b.r.x * AbsR[1][2] + b.r.z * AbsR[1][0];
if (Abs(t.x * R[2][1] - t.z * R[0][1]) > ra + rb)
return false;
// A.y <cross> B.z
ra = r.x * AbsR[2][2] + r.z * AbsR[0][2];
rb = b.r.x * AbsR[1][1] + b.r.y * AbsR[1][0];
if (Abs(t.x * R[2][2] - t.z * R[0][2]) > ra + rb)
return false;
// A.z <cross> B.x
ra = r.x * AbsR[1][0] + r.y * AbsR[0][0];
rb = b.r.y * AbsR[2][2] + b.r.z * AbsR[2][1];
if (Abs(t.y * R[0][0] - t.x * R[1][0]) > ra + rb)
return false;
// A.z <cross> B.y
ra = r.x * AbsR[1][1] + r.y * AbsR[0][1];
rb = b.r.x * AbsR[2][2] + b.r.z * AbsR[2][0];
if (Abs(t.y * R[0][1] - t.x * R[1][1]) > ra + rb)
return false;
// A.z <cross> B.z
ra = r.x * AbsR[1][2] + r.y * AbsR[0][2];
rb = b.r.x * AbsR[2][1] + b.r.y * AbsR[2][0];
if (Abs(t.y * R[0][2] - t.x * R[1][2]) > ra + rb)
return false;
// No separating axis exists, so the two OBB don't intersect.
return true;
}
/**
* Do texgen needed for glRasterPos.
* \param ctx rendering context
* \param vObj object-space vertex coordinate
* \param vEye eye-space vertex coordinate
* \param normal vertex normal
* \param unit texture unit number
* \param texcoord incoming texcoord and resulting texcoord
*/
static void
compute_texgen(struct gl_context *ctx, const GLfloat vObj[4], const GLfloat vEye[4],
const GLfloat normal[3], GLuint unit, GLfloat texcoord[4])
{
const struct gl_texture_unit *texUnit = &ctx->Texture.Unit[unit];
/* always compute sphere map terms, just in case */
GLfloat u[3], two_nu, rx, ry, rz, m, mInv;
COPY_3V(u, vEye);
NORMALIZE_3FV(u);
two_nu = 2.0F * DOT3(normal, u);
rx = u[0] - normal[0] * two_nu;
ry = u[1] - normal[1] * two_nu;
rz = u[2] - normal[2] * two_nu;
m = rx * rx + ry * ry + (rz + 1.0F) * (rz + 1.0F);
if (m > 0.0F)
mInv = 0.5F * (1.0f / sqrtf(m));
else
mInv = 0.0F;
if (texUnit->TexGenEnabled & S_BIT) {
switch (texUnit->GenS.Mode) {
case GL_OBJECT_LINEAR:
texcoord[0] = DOT4(vObj, texUnit->GenS.ObjectPlane);
break;
case GL_EYE_LINEAR:
texcoord[0] = DOT4(vEye, texUnit->GenS.EyePlane);
break;
case GL_SPHERE_MAP:
texcoord[0] = rx * mInv + 0.5F;
break;
case GL_REFLECTION_MAP:
texcoord[0] = rx;
break;
case GL_NORMAL_MAP:
texcoord[0] = normal[0];
break;
default:
_mesa_problem(ctx, "Bad S texgen in compute_texgen()");
return;
}
}
if (texUnit->TexGenEnabled & T_BIT) {
switch (texUnit->GenT.Mode) {
case GL_OBJECT_LINEAR:
texcoord[1] = DOT4(vObj, texUnit->GenT.ObjectPlane);
break;
case GL_EYE_LINEAR:
texcoord[1] = DOT4(vEye, texUnit->GenT.EyePlane);
break;
case GL_SPHERE_MAP:
texcoord[1] = ry * mInv + 0.5F;
break;
case GL_REFLECTION_MAP:
texcoord[1] = ry;
break;
case GL_NORMAL_MAP:
texcoord[1] = normal[1];
break;
default:
_mesa_problem(ctx, "Bad T texgen in compute_texgen()");
return;
}
}
if (texUnit->TexGenEnabled & R_BIT) {
switch (texUnit->GenR.Mode) {
case GL_OBJECT_LINEAR:
texcoord[2] = DOT4(vObj, texUnit->GenR.ObjectPlane);
break;
case GL_EYE_LINEAR:
texcoord[2] = DOT4(vEye, texUnit->GenR.EyePlane);
break;
case GL_REFLECTION_MAP:
texcoord[2] = rz;
break;
case GL_NORMAL_MAP:
texcoord[2] = normal[2];
break;
default:
_mesa_problem(ctx, "Bad R texgen in compute_texgen()");
return;
}
}
if (texUnit->TexGenEnabled & Q_BIT) {
switch (texUnit->GenQ.Mode) {
case GL_OBJECT_LINEAR:
texcoord[3] = DOT4(vObj, texUnit->GenQ.ObjectPlane);
break;
case GL_EYE_LINEAR:
texcoord[3] = DOT4(vEye, texUnit->GenQ.EyePlane);
break;
default:
_mesa_problem(ctx, "Bad Q texgen in compute_texgen()");
return;
}
}
}
/**
* Compute lighting for the raster position. RGB modes computed.
* \param ctx the context
* \param vertex vertex location
* \param normal normal vector
* \param Rcolor returned color
* \param Rspec returned specular color (if separate specular enabled)
*/
static void
shade_rastpos(struct gl_context *ctx,
const GLfloat vertex[4],
const GLfloat normal[3],
GLfloat Rcolor[4],
GLfloat Rspec[4])
{
/*const*/ GLfloat (*base)[3] = ctx->Light._BaseColor;
GLbitfield mask;
GLfloat diffuseColor[4], specularColor[4]; /* for RGB mode only */
COPY_3V(diffuseColor, base[0]);
diffuseColor[3] = CLAMP(
ctx->Light.Material.Attrib[MAT_ATTRIB_FRONT_DIFFUSE][3], 0.0F, 1.0F );
ASSIGN_4V(specularColor, 0.0, 0.0, 0.0, 1.0);
mask = ctx->Light._EnabledLights;
while (mask) {
const int i = u_bit_scan(&mask);
struct gl_light *light = &ctx->Light.Light[i];
GLfloat attenuation = 1.0;
GLfloat VP[3]; /* vector from vertex to light pos */
GLfloat n_dot_VP;
GLfloat diffuseContrib[3], specularContrib[3];
if (!(light->_Flags & LIGHT_POSITIONAL)) {
/* light at infinity */
COPY_3V(VP, light->_VP_inf_norm);
attenuation = light->_VP_inf_spot_attenuation;
}
else {
/* local/positional light */
GLfloat d;
/* VP = vector from vertex pos to light[i].pos */
SUB_3V(VP, light->_Position, vertex);
/* d = length(VP) */
d = (GLfloat) LEN_3FV( VP );
if (d > 1.0e-6F) {
/* normalize VP */
GLfloat invd = 1.0F / d;
SELF_SCALE_SCALAR_3V(VP, invd);
}
/* atti */
attenuation = 1.0F / (light->ConstantAttenuation + d *
(light->LinearAttenuation + d *
light->QuadraticAttenuation));
if (light->_Flags & LIGHT_SPOT) {
GLfloat PV_dot_dir = - DOT3(VP, light->_NormSpotDirection);
if (PV_dot_dir<light->_CosCutoff) {
continue;
}
else {
GLfloat spot = powf(PV_dot_dir, light->SpotExponent);
attenuation *= spot;
}
}
}
if (attenuation < 1e-3F)
continue;
n_dot_VP = DOT3( normal, VP );
if (n_dot_VP < 0.0F) {
ACC_SCALE_SCALAR_3V(diffuseColor, attenuation, light->_MatAmbient[0]);
continue;
}
/* Ambient + diffuse */
COPY_3V(diffuseContrib, light->_MatAmbient[0]);
ACC_SCALE_SCALAR_3V(diffuseContrib, n_dot_VP, light->_MatDiffuse[0]);
/* Specular */
{
const GLfloat *h;
GLfloat n_dot_h;
ASSIGN_3V(specularContrib, 0.0, 0.0, 0.0);
if (ctx->Light.Model.LocalViewer) {
GLfloat v[3];
COPY_3V(v, vertex);
NORMALIZE_3FV(v);
SUB_3V(VP, VP, v);
NORMALIZE_3FV(VP);
h = VP;
}
else if (light->_Flags & LIGHT_POSITIONAL) {
ACC_3V(VP, ctx->_EyeZDir);
NORMALIZE_3FV(VP);
h = VP;
}
else {
h = light->_h_inf_norm;
}
n_dot_h = DOT3(normal, h);
if (n_dot_h > 0.0F) {
GLfloat shine;
GLfloat spec_coef;
shine = ctx->Light.Material.Attrib[MAT_ATTRIB_FRONT_SHININESS][0];
spec_coef = powf(n_dot_h, shine);
if (spec_coef > 1.0e-10F) {
if (ctx->Light.Model.ColorControl==GL_SEPARATE_SPECULAR_COLOR) {
ACC_SCALE_SCALAR_3V( specularContrib, spec_coef,
light->_MatSpecular[0]);
}
else {
ACC_SCALE_SCALAR_3V( diffuseContrib, spec_coef,
light->_MatSpecular[0]);
}
}
}
}
ACC_SCALE_SCALAR_3V( diffuseColor, attenuation, diffuseContrib );
ACC_SCALE_SCALAR_3V( specularColor, attenuation, specularContrib );
}
Rcolor[0] = CLAMP(diffuseColor[0], 0.0F, 1.0F);
Rcolor[1] = CLAMP(diffuseColor[1], 0.0F, 1.0F);
Rcolor[2] = CLAMP(diffuseColor[2], 0.0F, 1.0F);
Rcolor[3] = CLAMP(diffuseColor[3], 0.0F, 1.0F);
Rspec[0] = CLAMP(specularColor[0], 0.0F, 1.0F);
Rspec[1] = CLAMP(specularColor[1], 0.0F, 1.0F);
Rspec[2] = CLAMP(specularColor[2], 0.0F, 1.0F);
Rspec[3] = CLAMP(specularColor[3], 0.0F, 1.0F);
}
static GLfloat opengl_direction_mul(GLfloat *d1, GLfloat *d2)
{
GLfloat direction = DOT3(d1, d2);
return( MAX2(direction, 0.0f));
}
float3 float3x3::operator *(const float3 &rhs) const
{
return float3(DOT3(v[0], rhs),
DOT3(v[1], rhs),
DOT3(v[2], rhs));
}
/**
* Determine type and flags from scratch.
*
* \param mat matrix.
*
* This is expensive enough to only want to do it once.
*/
static void analyse_from_scratch( GLmatrix *mat )
{
const GLfloat *m = mat->m;
GLuint mask = 0;
GLuint i;
for (i = 0 ; i < 16 ; i++) {
if (m[i] == 0.0) mask |= (1<<i);
}
if (m[0] == 1.0F) mask |= (1<<16);
if (m[5] == 1.0F) mask |= (1<<21);
if (m[10] == 1.0F) mask |= (1<<26);
if (m[15] == 1.0F) mask |= (1<<31);
mat->flags &= ~MAT_FLAGS_GEOMETRY;
/* Check for translation - no-one really cares
*/
if ((mask & MASK_NO_TRX) != MASK_NO_TRX)
mat->flags |= MAT_FLAG_TRANSLATION;
/* Do the real work
*/
if (mask == (GLuint) MASK_IDENTITY) {
mat->type = MATRIX_IDENTITY;
}
else if ((mask & MASK_2D_NO_ROT) == (GLuint) MASK_2D_NO_ROT) {
mat->type = MATRIX_2D_NO_ROT;
if ((mask & MASK_NO_2D_SCALE) != MASK_NO_2D_SCALE)
mat->flags |= MAT_FLAG_GENERAL_SCALE;
}
else if ((mask & MASK_2D) == (GLuint) MASK_2D) {
GLfloat mm = DOT2(m, m);
GLfloat m4m4 = DOT2(m+4,m+4);
GLfloat mm4 = DOT2(m,m+4);
mat->type = MATRIX_2D;
/* Check for scale */
if (SQ(mm-1) > SQ(1e-6) ||
SQ(m4m4-1) > SQ(1e-6))
mat->flags |= MAT_FLAG_GENERAL_SCALE;
/* Check for rotation */
if (SQ(mm4) > SQ(1e-6))
mat->flags |= MAT_FLAG_GENERAL_3D;
else
mat->flags |= MAT_FLAG_ROTATION;
}
else if ((mask & MASK_3D_NO_ROT) == (GLuint) MASK_3D_NO_ROT) {
mat->type = MATRIX_3D_NO_ROT;
/* Check for scale */
if (SQ(m[0]-m[5]) < SQ(1e-6) &&
SQ(m[0]-m[10]) < SQ(1e-6)) {
if (SQ(m[0]-1.0) > SQ(1e-6)) {
mat->flags |= MAT_FLAG_UNIFORM_SCALE;
}
}
else {
mat->flags |= MAT_FLAG_GENERAL_SCALE;
}
}
else if ((mask & MASK_3D) == (GLuint) MASK_3D) {
GLfloat c1 = DOT3(m,m);
GLfloat c2 = DOT3(m+4,m+4);
GLfloat c3 = DOT3(m+8,m+8);
GLfloat d1 = DOT3(m, m+4);
GLfloat cp[3];
mat->type = MATRIX_3D;
/* Check for scale */
if (SQ(c1-c2) < SQ(1e-6) && SQ(c1-c3) < SQ(1e-6)) {
if (SQ(c1-1.0) > SQ(1e-6))
mat->flags |= MAT_FLAG_UNIFORM_SCALE;
/* else no scale at all */
}
else {
mat->flags |= MAT_FLAG_GENERAL_SCALE;
}
/* Check for rotation */
if (SQ(d1) < SQ(1e-6)) {
CROSS3( cp, m, m+4 );
SUB_3V( cp, cp, (m+8) );
if (LEN_SQUARED_3FV(cp) < SQ(1e-6))
mat->flags |= MAT_FLAG_ROTATION;
else
mat->flags |= MAT_FLAG_GENERAL_3D;
}
else {
mat->flags |= MAT_FLAG_GENERAL_3D; /* shear, etc */
}
}
else if ((mask & MASK_PERSPECTIVE) == MASK_PERSPECTIVE && m[11]==-1.0F) {
mat->type = MATRIX_PERSPECTIVE;
mat->flags |= MAT_FLAG_GENERAL;
}
else {
mat->type = MATRIX_GENERAL;
mat->flags |= MAT_FLAG_GENERAL;
}
}
float noise3(float x, float y, float z) {
int c, o1[3], o2[3], g[4], I, J, K;
float f[4], noise[4] = {0.0f, 0.0f, 0.0f, 0.0f};
float s = (x + y + z) * F3;
float i = floorf(x + s);
float j = floorf(y + s);
float k = floorf(z + s);
float t = (i + j + k) * G3;
float pos[4][3];
pos[0][0] = x - (i - t);
pos[0][1] = y - (j - t);
pos[0][2] = z - (k - t);
if (pos[0][0] >= pos[0][1]) {
if (pos[0][1] >= pos[0][2]) {
ASSIGN(o1, 1, 0, 0);
ASSIGN(o2, 1, 1, 0);
} else if (pos[0][0] >= pos[0][2]) {
ASSIGN(o1, 1, 0, 0);
ASSIGN(o2, 1, 0, 1);
} else {
ASSIGN(o1, 0, 0, 1);
ASSIGN(o2, 1, 0, 1);
}
} else {
if (pos[0][1] < pos[0][2]) {
ASSIGN(o1, 0, 0, 1);
ASSIGN(o2, 0, 1, 1);
} else if (pos[0][0] < pos[0][2]) {
ASSIGN(o1, 0, 1, 0);
ASSIGN(o2, 0, 1, 1);
} else {
ASSIGN(o1, 0, 1, 0);
ASSIGN(o2, 1, 1, 0);
}
}
for (c = 0; c <= 2; c++) {
pos[3][c] = pos[0][c] - 1.0f + 3.0f * G3;
pos[2][c] = pos[0][c] - o2[c] + 2.0f * G3;
pos[1][c] = pos[0][c] - o1[c] + G3;
}
I = (int) i & 255;
J = (int) j & 255;
K = (int) k & 255;
g[0] = PERM[I + PERM[J + PERM[K]]] % 12;
g[1] = PERM[I + o1[0] + PERM[J + o1[1] + PERM[o1[2] + K]]] % 12;
g[2] = PERM[I + o2[0] + PERM[J + o2[1] + PERM[o2[2] + K]]] % 12;
g[3] = PERM[I + 1 + PERM[J + 1 + PERM[K + 1]]] % 12;
for (c = 0; c <= 3; c++) {
f[c] = 0.6f - pos[c][0] * pos[c][0] - pos[c][1] * pos[c][1] -
pos[c][2] * pos[c][2];
}
for (c = 0; c <= 3; c++) {
if (f[c] > 0) {
noise[c] = f[c] * f[c] * f[c] * f[c] * DOT3(pos[c], GRAD3[g[c]]);
}
}
return (noise[0] + noise[1] + noise[2] + noise[3]) * 32.0f;
}
int CheckCollisionGround( vector<z_face> &collision, vec center, float radius, float angle, float mindist)
{
float ground_skew = (float)cos(angle*PI180)*radius;
vec vpold_vp = mindist*vec(0,-1,0);
for( int i=0; i<collision.size(); i++)
{
vec normal = collision[i].normal;
float distance = PlaneDistance( normal, collision[i].a); // D = - (Ax+By+Cz)
// ---------------------------------------------------------
// najdeme kolidujuci bod na guli
vec ClosestPointOnSphere; // najblizsi bod gule k rovine aktualneho trojuholnika
// najdeme ho ako priesecnik priamky prechadzajucej stredom gule a smerovym vektorom = normalovemu vektoru roviny (trojuholnika)
// vypocitame ho, ale jednodusie tak, ze pripocitame (opocitame) k stredu vektor normala*radius
if( PlanePointDelta( normal, distance, center) > 0 )
{
// center je na strane normaly, blizsi bod je v opacnom smere ako smer normaly
ClosestPointOnSphere = -radius*normal+center;
}
else
{
// center je na opacnej strane ako normala, blizsi bod je v smere normaly
ClosestPointOnSphere = radius*normal+center;
}
// ---------------------------------------------------------
// najdeme kolidujuci bod na trojuholniku
// najprv najdeme kolidujuci bod vzhladom na rovinu v ktorej lezi trojuholnik
vec contactPointSphereToPlane; // kolidujuci bod na rovine trojuholnika s gulou
float distanceCenterToPlane; // vzdialenost stredu gule k rovine
// zistime ci rovina pretina gulu
if( SpherePlaneCollision( center, 0.9999f*radius, normal, distance, &distanceCenterToPlane)==1 )
{
// gula pretina rovinu
// kolidujuci bod je bod na rovine najblizsi k stredu gule
// je vzdialeny od roviny na distanceCenterToPlane, pretoze pocitame bod na rovine pouzijeme -
contactPointSphereToPlane = center-distanceCenterToPlane*normal;
}
else
{
// nie sme v kolizii z gulov, ak sa pohybujeme v smere od roviny, nemoze nastat ziadna kolizia
// ak sa pohybujeme v smere kolmom na normalovy vektor roviny, tak isto kolizia nehrozi
// kvoli nepresnosti vypoctov umoznime pohyb aj ked velmi malou castou smeruje do roviny
// if( DOT3( vpold_vp, center-ClosestPointOnSphere) >= 0)
if( DOT3( vpold_vp, center-ClosestPointOnSphere) > -0.000001f)
{
continue;
}
// gula nepretina rovinu
// kolidujuci bod je priesecnik roviny a priamky vedenej z bodu ClosestPointOnSphere
// v smere pohybu t.j. z vpold do vp
float t = LinePlaneIntersectionDirParameter( ClosestPointOnSphere, vpold_vp, normal, distance);
// t > 1.f, priesecnik z rovinou je dalej ako vpold_vp od bodu ClosestPointOnSphere
if(t>1.f)
continue; // za cely krok vpold_vp sa s tymto trojuholnikom nestretneme
else if( t<-radius/vpold_vp.Length()) // priesecnik je za gulou, v smere pohybu tuto rovinu nestretneme
continue;
else
contactPointSphereToPlane = ClosestPointOnSphere+t*vpold_vp;
}
// najdeme kolidujuci bod na trojuholniku
vec contactPointSphereToTriangle;
// ak sa bod contactPointSphereToPlane nenachadza v trojuholniku
// najdeme najblizsi bod trojuholnika k bodu contactPointSphereToTriangle
if( !PointInsidePolygon( contactPointSphereToPlane, collision[i].a, collision[i].b, collision[i].c) )
{
// najdeme najblizsi bod k contactPointSphereToPlane na hranach trojuholnika
// z tychto vyberieme najblizi k contactPointSphereToPlane
vec closest_ab = ClosestPointOnLine( collision[i].a, collision[i].b, contactPointSphereToPlane);
vec closest_bc = ClosestPointOnLine( collision[i].b, collision[i].c, contactPointSphereToPlane);
vec closest_ca = ClosestPointOnLine( collision[i].c, collision[i].a, contactPointSphereToPlane);
float dist_ab = Distance2( closest_ab, contactPointSphereToPlane);
float dist_bc = Distance2( closest_bc, contactPointSphereToPlane);
float dist_ca = Distance2( closest_ca, contactPointSphereToPlane);
if( dist_ab<dist_bc)
{
if(dist_ab<dist_ca)
contactPointSphereToTriangle = closest_ab;
else
contactPointSphereToTriangle = closest_ca;
}
else
{
if(dist_bc<dist_ca)
contactPointSphereToTriangle = closest_bc;
else
contactPointSphereToTriangle = closest_ca;
}
// kedze kolidujuci bod na trojuholniku je iny ako kolidujuci bod na rovine
// zmeni sa aj kolidujuci bod na guli - ClosestPointOnSphere
// vypocitame ho ako priesecnik gule a priamky z bodu contactPointSphereToTriangle
// v smere pohybu t.j. z vpold do vp
double t1,t2;
if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
ClosestPointOnSphere = t1*vpold_vp+contactPointSphereToTriangle;
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else if( t1>0 && t2<0)
{
// gula je v stene, vratime ju von zo steny
// berieme bod, ktory je blizsie k rovine
vec t1point = t1*vpold_vp+contactPointSphereToTriangle;
vec t2point = t2*vpold_vp+contactPointSphereToTriangle;
/* if(PlanePointDistance( normal, distance, t1point)<=PlanePointDistance( normal, distance, t2point) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
*/
if( ABS(t1) < ABS(t2) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// bod je vnutri trojuholnika
// contactPointSphereToTriangle = contactPointSphereToPlane;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
/* if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
return 1;
}
else if( t1>0 && t2<0)
{
return 1; // gula je v stene
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
return 1; // bod je vnutri trojuholnika
}*/
}
return 0;
}
vec CheckCollision( vector<z_face> &collision, vec vp, vec vpold, float radius)
{
// if(vpold_vp.Length()==0)return vpold;
vec vpold_vp = vp-vpold; // smer pohybu
vec vpold_vp_povodny = vpold_vp;// smer pohybu
int iter=0;
float radius2 = radius*radius;
vec newmove = vpold_vp;
vec newClosestPointOnSphere;
vec newContactPointSphereToTriangle;
do
{
float distanceCenterPointOnTriangle=1.e+15f;
int smykanie=0;
for( int i=0; i<collision.size(); i++)
{
vec normal = collision[i].normal;
vec vp_move;
vec center = vpold;
float distance = PlaneDistance( normal, collision[i].a); // D = - (Ax+By+Cz)
// ---------------------------------------------------------
// najdeme kolidujuci bod na guli
vec ClosestPointOnSphere; // najblizsi bod gule k rovine aktualneho trojuholnika
// najdeme ho ako priesecnik priamky prechadzajucej stredom gule a smerovym vektorom = normalovemu vektoru roviny (trojuholnika)
// vypocitame ho, ale jednodusie tak, ze pripocitame (opocitame) k stredu vektor normala*radius
if( PlanePointDelta( normal, distance, center) > 0 )
{
// center je na strane normaly, blizsi bod je v opacnom smere ako smer normaly
ClosestPointOnSphere = -radius*normal+center;
}
else
{
// center je na opacnej strane ako normala, blizsi bod je v smere normaly
ClosestPointOnSphere = radius*normal+center;
}
// ---------------------------------------------------------
// najdeme kolidujuci bod na trojuholniku
// najprv najdeme kolidujuci bod vzhladom na rovinu v ktorej lezi trojuholnik
vec contactPointSphereToPlane; // kolidujuci bod na rovine trojuholnika s gulou
float distanceCenterToPlane; // vzdialenost stredu gule k rovine
// zistime ci rovina pretina gulu
if( SpherePlaneCollision( center, 0.9999f*radius, normal, distance, &distanceCenterToPlane)==1 )
{
// gula pretina rovinu
// kolidujuci bod je bod na rovine najblizsi k stredu gule
// je vzdialeny od roviny na distanceCenterToPlane, pretoze pocitame bod na rovine pouzijeme -
contactPointSphereToPlane = center-distanceCenterToPlane*normal;
}
else
{
// nie sme v kolizii z gulov, ak sa pohybujeme v smere od roviny, nemoze nastat ziadna kolizia
// ak sa pohybujeme v smere kolmom na normalovy vektor roviny, tak isto kolizia nehrozi
// kvoli nepresnosti vypoctov umoznime pohyb aj ked velmi malou castou smeruje do roviny
// if( DOT3( vpold_vp, center-ClosestPointOnSphere) >= 0)
if( DOT3( vpold_vp, center-ClosestPointOnSphere) > -0.000001f)
{
continue;
}
// gula nepretina rovinu
// kolidujuci bod je priesecnik roviny a priamky vedenej z bodu ClosestPointOnSphere
// v smere pohybu t.j. z vpold do vp
float t = LinePlaneIntersectionDirParameter( ClosestPointOnSphere, vpold_vp, normal, distance);
// t > 1.f, priesecnik z rovinou je dalej ako vpold_vp od bodu ClosestPointOnSphere
if(t>1.f)
continue; // za cely krok vpold_vp sa s tymto trojuholnikom nestretneme
else if( t<-radius/vpold_vp.Length()) // priesecnik je za gulou, v smere pohybu tuto rovinu nestretneme
continue;
else
contactPointSphereToPlane = ClosestPointOnSphere+t*vpold_vp;
}
// najdeme kolidujuci bod na trojuholniku
vec contactPointSphereToTriangle;
// ak sa bod contactPointSphereToPlane nenachadza v trojuholniku
// najdeme najblizsi bod trojuholnika k bodu contactPointSphereToTriangle
if( !PointInsidePolygon( contactPointSphereToPlane, collision[i].a, collision[i].b, collision[i].c) )
{
// najdeme najblizsi bod k contactPointSphereToPlane na hranach trojuholnika
// z tychto vyberieme najblizi k contactPointSphereToPlane
vec closest_ab = ClosestPointOnLine( collision[i].a, collision[i].b, contactPointSphereToPlane);
vec closest_bc = ClosestPointOnLine( collision[i].b, collision[i].c, contactPointSphereToPlane);
vec closest_ca = ClosestPointOnLine( collision[i].c, collision[i].a, contactPointSphereToPlane);
float dist_ab = Distance2( closest_ab, contactPointSphereToPlane);
float dist_bc = Distance2( closest_bc, contactPointSphereToPlane);
float dist_ca = Distance2( closest_ca, contactPointSphereToPlane);
if( dist_ab<dist_bc)
{
if(dist_ab<dist_ca)
contactPointSphereToTriangle = closest_ab;
else
contactPointSphereToTriangle = closest_ca;
}
else
{
if(dist_bc<dist_ca)
contactPointSphereToTriangle = closest_bc;
else
contactPointSphereToTriangle = closest_ca;
}
// kedze kolidujuci bod na trojuholniku je iny ako kolidujuci bod na rovine
// zmeni sa aj kolidujuci bod na guli - ClosestPointOnSphere
// vypocitame ho ako priesecnik gule a priamky z bodu contactPointSphereToTriangle
// v smere pohybu t.j. z vpold do vp
double t1,t2;
if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
ClosestPointOnSphere = t1*vpold_vp+contactPointSphereToTriangle;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else if( t1>0 && t2<0)
{
// gula je v stene, vratime ju von zo steny
// berieme bod, ktory je blizsie k rovine
vec t1point = t1*vpold_vp+contactPointSphereToTriangle;
vec t2point = t2*vpold_vp+contactPointSphereToTriangle;
/* if(PlanePointDistance( normal, distance, t1point)<=PlanePointDistance( normal, distance, t2point) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
*/
if( ABS(t1) < ABS(t2) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
// bod je vnutri trojuholnika
contactPointSphereToTriangle = contactPointSphereToPlane;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
// zistime vzdialenost kontaktneho bodu na trojuholniku ku stredu gule
float dist = Distance2(contactPointSphereToTriangle,center);
if(dist<radius2) // ak je mensi ako polomer, gula je v kolizii z polygonom
{
if(dist<distanceCenterPointOnTriangle) // ak vzdialenost je mensia ako ineho bodu v kolizii, nahradime ho
{
distanceCenterPointOnTriangle=dist;
newmove = vp_move;
newClosestPointOnSphere = ClosestPointOnSphere;
newContactPointSphereToTriangle = contactPointSphereToTriangle;
}
}
else
{
if(distanceCenterPointOnTriangle>5.e+14f) // nenasiel sa ziaden bod vnutri gule
{
if( vp_move.Length2() < newmove.Length2() ) // berieme kratsi
{
newmove = vp_move;
newClosestPointOnSphere = ClosestPointOnSphere;
newContactPointSphereToTriangle = contactPointSphereToTriangle;
}
}
}
smykanie=1;
}
if(smykanie)
{
vec normal=vpold-newClosestPointOnSphere;
float distance = PlaneDistance( normal, newContactPointSphereToTriangle);
vec delta = LinePlaneIntersectionDir( newClosestPointOnSphere+vpold_vp, normal, normal, distance)-newContactPointSphereToTriangle;
// vec newvp = newClosestPointOnSphere+vpold_vp;
// float distancepoint = PlanePointDelta( normal, distance, newvp);
// vec intersec = -distancepoint*normal+newvp;
// vec delta = intersec-newContactPointSphereToTriangle;
// taky klzavy pohyb, ktory ide proti povodnemu pohybu zamietneme
if( DOT3(vpold_vp_povodny, delta) < 0)delta.clear();
vpold += newmove; // posunieme sa po najblizi kolidujuci bod
vpold += 0.000001f*normal;
vp = vpold + delta; // cielovy bod posunieme o deltu klzanim
vpold_vp = vp-vpold; // novy vektor pohybu
newmove = vpold_vp;
iter++;
}
else
{
vpold += newmove;
vpold_vp.clear();
iter=1000;
}
}
while( (vpold_vp.Length2()>1.e-8f)&&(iter<10) );
return vpold;
}
