bool RayCast_Vector( ref btTransform Pos, ref btVector3 Vector, ref RayCast_in In, ref RayCast_out Out, bool InvertCollision )
{
return RayCast_Vector( ref Pos.m_origin, ref Vector, ref In, ref Out, InvertCollision );
}
bool RayCast_Vector( ref btVector3 Pos, ref btVector3 Vector, ref RayCast_in In, ref RayCast_out Out, bool InvertCollision )
{
btVector3 Delta_h, Delta_v, Delta_s;
btVector3 Offset_h = btVector3.Zero, Offset_v = btVector3.Zero, Offset_s = btVector3.Zero;
btVector3 Norm_h = btVector3.Zero, Norm_v = btVector3.Zero, Norm_s = btVector3.Zero;
btVector3 Collision_h = btVector3.Zero, Collision_v = btVector3.Zero, Collision_s = btVector3.Zero;
int ActualCube_x_h, ActualCube_y_h, ActualCube_z_h;
int ActualCube_x_v, ActualCube_y_v, ActualCube_z_v;
int ActualCube_x_s, ActualCube_y_s, ActualCube_z_s;
int NewCube_x_h, NewCube_y_h, NewCube_z_h;
int NewCube_x_v, NewCube_y_v, NewCube_z_v;
int NewCube_x_s, NewCube_y_s, NewCube_z_s;
bool Collide_X, Collide_Y, Collide_Z;
int i;
byte Face_h, Face_s, Face_v;
ushort VoxelType;
btVector3 Norm;
btVector3 Tmp;
float Vector_Len;
// Normalize input vector.
//Vector_Len = Math.Sqrt( Vector.x * Vector.x + Vector.y * Vector.y + Vector.z * Vector.z);
Norm = Vector;// / Vector_Len;
// Norm = Vector;
//printf("Norm:%lf %lf %lf\n",Norm.x,Norm.y,Norm.z);
Collide_X = Collide_Y = Collide_Z = false;
Face_h = Face_s = Face_v = 0;
if( Norm.x > 0.00000001 )
{
Face_h = 2;
Collide_X = true;
Delta_h.y = Norm.y / Norm.x;
Delta_h.z = Norm.z / Norm.x;
Delta_h.w = 1.0f / Norm.x;
Collision_h.x = (float)( Math.Floor( Pos.x / VoxelBlockSize ) + 1.0f ) * VoxelBlockSize;
Collision_h.y = ( Collision_h.x - Pos.x ) * Delta_h.y + Pos.y;
Collision_h.z = ( Collision_h.x - Pos.x ) * Delta_h.z + Pos.z;
Collision_h.w = ( Collision_h.x - Pos.x ) * Delta_h.w;
Offset_h.x = VoxelBlockSize;
Offset_h.y = Delta_h.y * VoxelBlockSize;
Offset_h.z = Delta_h.z * VoxelBlockSize;
Offset_h.w = Delta_h.w * VoxelBlockSize;
Norm_h.x = Offset_h.x / ( VoxelBlockSize / 2 );
Norm_h.y = Offset_h.y / ( VoxelBlockSize / 2 );
Norm_h.z = Offset_h.z / ( VoxelBlockSize / 2 );
}
else if( Norm.x < -0.00000001 )
{
Face_h = 3;
Collide_X = true;
Delta_h.y = Norm.y / -Norm.x;
Delta_h.z = Norm.z / -Norm.x;
Delta_h.w = 1.0f / Math.Abs( Norm.x );
Collision_h.x = (float)( Math.Floor( Pos.x / VoxelBlockSize ) ) * VoxelBlockSize; // - 1.0
Collision_h.y = ( Pos.x - Collision_h.x ) * Delta_h.y + Pos.y;
Collision_h.z = ( Pos.x - Collision_h.x ) * Delta_h.z + Pos.z;
Collision_h.w = ( Pos.x - Collision_h.x ) * Delta_h.w;
Offset_h.x = -VoxelBlockSize;
Offset_h.y = Delta_h.y * VoxelBlockSize;
Offset_h.z = Delta_h.z * VoxelBlockSize;
Offset_h.w = Delta_h.w * VoxelBlockSize;
Norm_h.x = Offset_h.x / ( VoxelBlockSize / 2 );
Norm_h.y = Offset_h.y / ( VoxelBlockSize / 2 );
Norm_h.z = Offset_h.z / ( VoxelBlockSize / 2 );
}
if( Norm.y > 0.00000001 )
{
Face_v = 5;
Collide_Y = true;
Delta_v.x = Norm.x / Norm.y;
Delta_v.z = Norm.z / Norm.y;
Delta_v.w = 1 / Norm.y;
Collision_v.y = (float)( Math.Floor( Pos.y / VoxelBlockSize ) + 1.0 ) * VoxelBlockSize;
Collision_v.x = ( Collision_v.y - Pos.y ) * Delta_v.x + Pos.x;
Collision_v.z = ( Collision_v.y - Pos.y ) * Delta_v.z + Pos.z;
Collision_v.w = ( Collision_v.y - Pos.y ) * Delta_v.w;
Offset_v.y = VoxelBlockSize;
Offset_v.x = Delta_v.x * VoxelBlockSize;
Offset_v.z = Delta_v.z * VoxelBlockSize;
Offset_v.w = Delta_v.w * VoxelBlockSize;
Norm_v.x = Offset_v.x / ( VoxelBlockSize / 2 );
Norm_v.y = Offset_v.y / ( VoxelBlockSize / 2 );
Norm_v.z = Offset_v.z / ( VoxelBlockSize / 2 );
}
else if( Norm.y < -0.00000001 )
{
Face_v = 4;
Collide_Y = true;
Delta_v.x = Norm.x / -Norm.y;
Delta_v.z = Norm.z / -Norm.y;
Delta_v.w = 1.0f / -Norm.y;
Collision_v.y = ( (float)Math.Floor( Pos.y / VoxelBlockSize ) ) * VoxelBlockSize; // - 1.0
Collision_v.x = ( Pos.y - Collision_v.y ) * Delta_v.x + Pos.x;
Collision_v.z = ( Pos.y - Collision_v.y ) * Delta_v.z + Pos.z;
Collision_v.w = Math.Abs( ( Collision_v.y - Pos.y ) * Delta_v.w );
Offset_v.y = -VoxelBlockSize;
Offset_v.x = Delta_v.x * VoxelBlockSize;
Offset_v.z = Delta_v.z * VoxelBlockSize;
Offset_v.w = Delta_v.w * VoxelBlockSize;
Norm_v.x = Offset_v.x / ( VoxelBlockSize / 2 );
Norm_v.y = Offset_v.y / ( VoxelBlockSize / 2 );
Norm_v.z = Offset_v.z / ( VoxelBlockSize / 2 );
}
if( Norm.z > 0.00000001 )
{
Face_s = 0;
Collide_Z = true;
Delta_s.x = Norm.x / Norm.z;
Delta_s.y = Norm.y / Norm.z;
Delta_s.w = 1.0f / Norm.z;
Collision_s.z = ( (float)Math.Floor( Pos.z / VoxelBlockSize + 1.0 ) ) * VoxelBlockSize;
Collision_s.x = ( Collision_s.z - Pos.z ) * Delta_s.x + Pos.x;
Collision_s.y = ( Collision_s.z - Pos.z ) * Delta_s.y + Pos.y;
Collision_s.w = ( Collision_s.z - Pos.z ) * Delta_s.w;
Offset_s.z = VoxelBlockSize;
Offset_s.x = Delta_s.x * VoxelBlockSize;
Offset_s.y = Delta_s.y * VoxelBlockSize;
Offset_s.w = Delta_s.w * VoxelBlockSize;
Norm_s.x = Offset_s.x / ( VoxelBlockSize / 2 );
Norm_s.y = Offset_s.y / ( VoxelBlockSize / 2 );
Norm_s.z = Offset_s.z / ( VoxelBlockSize / 2 );
}
else if( Norm.z < -0.00000001 )
{
Face_s = 1;
Collide_Z = true;
Delta_s.x = Norm.x / -Norm.z;
Delta_s.y = Norm.y / -Norm.z;
Delta_s.w = 1.0f / -Norm.z;
Collision_s.z = ( (float)Math.Floor( Pos.z / VoxelBlockSize ) ) * VoxelBlockSize; // - 1.0
Collision_s.x = ( Pos.z - Collision_s.z ) * Delta_s.x + Pos.x;
Collision_s.y = ( Pos.z - Collision_s.z ) * Delta_s.y + Pos.y;
Collision_s.w = ( Pos.z - Collision_s.z ) * Delta_s.w;
Offset_s.z = -VoxelBlockSize;
Offset_s.x = Delta_s.x * VoxelBlockSize;
Offset_s.y = Delta_s.y * VoxelBlockSize;
Offset_s.w = Delta_s.w * VoxelBlockSize;
Norm_s.x = Offset_s.x / ( VoxelBlockSize / 2 );
Norm_s.y = Offset_s.y / ( VoxelBlockSize / 2 );
Norm_s.z = Offset_s.z / ( VoxelBlockSize / 2 );
}
/*
printf ("Loc(%lf %lf %lf) Norm(%lf %lf %lf) Col(%lf %lf %lf %lf) Off(%lf %lf %lf %lf) C(%d,%d,%d)\n", Pos.x, Pos.y, Pos.z, Norm.x,Norm.y, Norm.z, Collision_s.x, Collision_s.y, Collision_s.z, Collision_s.w, Offset_s.x,Offset_s.y, Offset_s.z,Offset_s.w
,(ushort)((Collide_X==true)? 1:0) ,(ushort)((Collide_Y==true)? 1:0), (ushort)((Collide_Z==true)? 1:0));
*/
// printf("yaw: %04lf pitch: %lf Offset_y:%lf Offset_z:%lf xyz:%lf %lf %lf NXYZ:%lf %lf %lf Dxyz:%lf %lf %lf", yaw,pitch, Delta_h.y, Delta_h.z,x,y,z, Norm_h.x, Norm_h.y, Norm_h.z, Delta_h.x, Delta_h.y, Delta_h.z);
//printf("Angle (y:%lf p:%lf) XYZ:(%lf %lf %lf) Off(%lf %lf %lf %lf) Coll(%lf %lf %lf %lf) Norm(%lg %lg %lf) :\n", yaw,pitch,x,y,z, Offset_s.x, Offset_s.y, Offset_s.z, Offset_s.w, Collision_s.x, Collision_s.y, Collision_s.z, Collision_s.w, Norm_s.x,Norm_s.y, Norm_s.z);
int Match_h = 0;
int Match_s = 0;
int Match_v = 0;
int Cycle = 1;
float MinW = 10000000.0f;
for( i = 0; i < In.MaxCubeIterations; i++ )
{
// if ( (Collision_h.w > In.MaxDetectionDistance) || (Collision_s.w > In.MaxDetectionDistance) || (Collision_v.w > In.MaxDetectionDistance)) { Out.Collided = false; return(false); }
// Horizontal x axis.
if( Collide_X )
{
if( Match_h == 0 && Collision_h.w < MinW )
{
ActualCube_x_h = (int)Math.Floor( ( Collision_h.x - Norm_h.x ) / VoxelBlockSize );
ActualCube_y_h = (int)Math.Floor( ( Collision_h.y - Norm_h.y ) / VoxelBlockSize );
ActualCube_z_h = (int)Math.Floor( ( Collision_h.z - Norm_h.z ) / VoxelBlockSize );
NewCube_x_h = (int)Math.Floor( ( Collision_h.x ) / VoxelBlockSize );
NewCube_y_h = (int)Math.Floor( ( Collision_h.y ) / VoxelBlockSize );
NewCube_z_h = (int)Math.Floor( ( Collision_h.z ) / VoxelBlockSize );
//ActualCube_x = (int)Math.Floor((Collision_h.x - Norm_h.x) / VoxelBlockSize); ActualCube_y = (int)Math.Floor((Collision_h.y - Norm_h.y) / VoxelBlockSize); ActualCube_z = (int)Math.Floor((Collision_h.z - Norm_h.z) / VoxelBlockSize);
//NewCube_x = (int)Math.Floor((Collision_h.x + Norm_h.x) / VoxelBlockSize); NewCube_y = (int)Math.Floor((Collision_h.y + Norm_h.y) / VoxelBlockSize); NewCube_z = (int)Math.Floor((Collision_h.z + Norm_h.z) / VoxelBlockSize);
if( Face_h == 3 ) NewCube_x_h--;
VoxelType = GetVoxel( NewCube_x_h, NewCube_y_h, NewCube_z_h );
if( !VoxelTypeManager.VoxelTable[VoxelType].properties.Is_PlayerCanPassThrough ^ InvertCollision )
{
Out.PredPointedVoxel.X = ActualCube_x_h; Out.PredPointedVoxel.Y = ActualCube_y_h; Out.PredPointedVoxel.Z = ActualCube_z_h;
Out.PointedVoxel.X = NewCube_x_h; Out.PointedVoxel.Y = NewCube_y_h; Out.PointedVoxel.Z = NewCube_z_h;
Out.CollisionPoint.x = Collision_h.x; Out.CollisionPoint.y = Collision_h.y; Out.CollisionPoint.z = Collision_h.z; Out.CollisionDistance = Collision_h.w;
Out.CollisionAxe = 0; Out.CollisionFace = Face_h;
Out.PointInCubeFace.X = ( Out.CollisionPoint.z % VoxelBlockSize ); Out.PointInCubeFace.Y = ( Out.CollisionPoint.y % VoxelBlockSize );
//printf(" MATCH_H: %lf (%ld %ld %ld) C:%ld\n",Collision_h.w, NewCube_x_h, NewCube_y_h, NewCube_z_h, Cycle);
Match_h = Cycle;
MinW = Collision_h.w;
}
}
}
// Horizontal z axis.
if( Collide_Z )
{
if( Match_s == 0 && Collision_s.w < MinW )
{
ActualCube_x_s = (int)Math.Floor( ( Collision_s.x - Norm_s.x ) / VoxelBlockSize ); ActualCube_y_s = (int)Math.Floor( ( Collision_s.y - Norm_s.y ) / VoxelBlockSize ); ActualCube_z_s = (int)Math.Floor( ( Collision_s.z - Norm_s.z ) / VoxelBlockSize );
NewCube_x_s = (int)Math.Floor( ( Collision_s.x ) / VoxelBlockSize ); NewCube_y_s = (int)Math.Floor( ( Collision_s.y ) / VoxelBlockSize ); NewCube_z_s = (int)Math.Floor( ( Collision_s.z ) / VoxelBlockSize );
//ActualCube_x_s = (int)Math.Floor((Collision_s.x - Norm_s.x) / VoxelBlockSize); ActualCube_y_s = (int)Math.Floor((Collision_s.y - Norm_s.y) / VoxelBlockSize); ActualCube_z_s = (int)Math.Floor((Collision_s.z - Norm_s.z) / VoxelBlockSize);
//NewCube_x_s = (int)Math.Floor((Collision_s.x + Norm_s.x) / VoxelBlockSize); NewCube_y_s = (int)Math.Floor((Collision_s.y + Norm_s.y) / VoxelBlockSize); NewCube_z_s = (int)Math.Floor((Collision_s.z + Norm_s.z) / VoxelBlockSize);
if( Face_s == 1 ) NewCube_z_s--;
VoxelType = GetVoxel( NewCube_x_s, NewCube_y_s, NewCube_z_s );
if( !VoxelTypeManager.VoxelTable[VoxelType].properties.Is_PlayerCanPassThrough ^ InvertCollision )
{
Out.PredPointedVoxel.X = ActualCube_x_s; Out.PredPointedVoxel.Y = ActualCube_y_s; Out.PredPointedVoxel.Z = ActualCube_z_s;
Out.PointedVoxel.X = NewCube_x_s; Out.PointedVoxel.Y = NewCube_y_s; Out.PointedVoxel.Z = NewCube_z_s;
Out.CollisionPoint.x = Collision_s.x; Out.CollisionPoint.y = Collision_s.y; Out.CollisionPoint.z = Collision_s.z; Out.CollisionDistance = Collision_s.w;
Out.CollisionAxe = 2; Out.CollisionFace = Face_s;
Out.PointInCubeFace.X = ( Out.CollisionPoint.x % VoxelBlockSize ); Out.PointInCubeFace.Y = ( Out.CollisionPoint.y % VoxelBlockSize );
//printf(" MATCH_S: %lf (%ld %ld %ld) C:%ld\n",Collision_s.w, NewCube_x_s, NewCube_y_s, NewCube_z_s, Cycle);
Match_s = Cycle;
MinW = Collision_s.w;
}
}
}
// Vertical y axis.
if( Collide_Y )
{
if( Match_v == 0 && Collision_v.w < MinW )
{
ActualCube_x_v = (int)Math.Floor( ( Collision_v.x - Norm_v.x ) / VoxelBlockSize ); ActualCube_y_v = (int)Math.Floor( ( Collision_v.y - Norm_v.y ) / VoxelBlockSize ); ActualCube_z_v = (int)Math.Floor( ( Collision_v.z - Norm_v.z ) / VoxelBlockSize );
NewCube_x_v = (int)Math.Floor( ( Collision_v.x ) / VoxelBlockSize ); NewCube_y_v = (int)Math.Floor( ( Collision_v.y ) / VoxelBlockSize ); NewCube_z_v = (int)Math.Floor( ( Collision_v.z ) / VoxelBlockSize );
//ActualCube_x_v = (int)Math.Floor((Collision_v.x - Norm_v.x) / VoxelBlockSize); ActualCube_y_v = (int)Math.Floor((Collision_v.y - Norm_v.y) / VoxelBlockSize); ActualCube_z_v = (int)Math.Floor((Collision_v.z - Norm_v.z) / VoxelBlockSize);
//NewCube_x_v = (int)Math.Floor((Collision_v.x + Norm_v.x) / VoxelBlockSize); NewCube_y_v = (int)Math.Floor((Collision_v.y + Norm_v.y) / VoxelBlockSize); NewCube_z_v = (int)Math.Floor((Collision_v.z + Norm_v.z) / VoxelBlockSize);
if( Face_v == 4 ) NewCube_y_v--;
VoxelType = GetVoxel( NewCube_x_v, NewCube_y_v, NewCube_z_v );
if( !VoxelTypeManager.VoxelTable[VoxelType].properties.Is_PlayerCanPassThrough ^ InvertCollision )
{
Out.PointedVoxel.X = NewCube_x_v; Out.PointedVoxel.Y = NewCube_y_v; Out.PointedVoxel.Z = NewCube_z_v;
Out.CollisionPoint.x = Collision_v.x; Out.CollisionPoint.y = Collision_v.y; Out.CollisionPoint.z = Collision_v.z; Out.CollisionDistance = Collision_v.w;
Out.PredPointedVoxel.X = ActualCube_x_v; Out.PredPointedVoxel.Y = ActualCube_y_v; Out.PredPointedVoxel.Z = ActualCube_z_v;
Out.CollisionAxe = 1; Out.CollisionFace = Face_v;
Out.PointInCubeFace.X = ( Out.CollisionPoint.x % VoxelBlockSize ); Out.PointInCubeFace.Y = ( Out.CollisionPoint.z % VoxelBlockSize );
//printf(" MATCH_V: %lf (%ld %ld %ld) C:%ld\n",Collision_v.w, NewCube_x_v, NewCube_y_v, NewCube_z_v,Cycle );
Match_v = Cycle;
MinW = Collision_v.w;
}
}
}
//printf(" Match (H:%lf S:%lf V:%lf) \n", Collision_h.w, Collision_s.w, Collision_v.w);
if( Match_h > 0 && ( Cycle - Match_h ) > In.PlaneCubeDiff ) { Out.Collided = true; return ( true ); }
if( Match_s > 0 && ( Cycle - Match_s ) > In.PlaneCubeDiff ) { Out.Collided = true; return ( true ); }
if( Match_v > 0 && ( Cycle - Match_v ) > In.PlaneCubeDiff ) { Out.Collided = true; return ( true ); }
if( Collide_X )
Collision_h.x += Offset_h.x; Collision_h.y += Offset_h.y; Collision_h.z += Offset_h.z; Collision_h.w += Offset_h.w;
if( Collide_Y )
Collision_v.x += Offset_v.x; Collision_v.y += Offset_v.y; Collision_v.z += Offset_v.z; Collision_v.w += Offset_v.w;
if( Collide_Z )
Collision_s.x += Offset_s.x; Collision_s.y += Offset_s.y; Collision_s.z += Offset_s.z; Collision_s.w += Offset_s.w;
Cycle++;
}
// compute it as a delta and revert this change :/
if( false )
{
if( ( !Collide_Y && Collide_X ) || ( Collide_Y && Collide_X && ( Collision_h.w < Collision_v.w ) ) )
{
if( Collide_Z && Collision_h.w < Collision_s.w )
{
Out.PredPointedVoxel.X = ActualCube_x_h; Out.PredPointedVoxel.Y = ActualCube_y_h; Out.PredPointedVoxel.Z = ActualCube_z_h;
Out.PointedVoxel.X = NewCube_x_h; Out.PointedVoxel.Y = NewCube_y_h; Out.PointedVoxel.Z = NewCube_z_h;
}
else
{
if( ( Collide_Z && !Collide_Y ) || ( Collide_Z && Collide_Y && Collision_s.w < Collision_v.w ) )
{
Out.PredPointedVoxel.X = ActualCube_x_s; Out.PredPointedVoxel.Y = ActualCube_y_s; Out.PredPointedVoxel.Z = ActualCube_z_s;
Out.PointedVoxel.X = NewCube_x_s; Out.PointedVoxel.Y = NewCube_y_s; Out.PointedVoxel.Z = NewCube_z_s;
}
else
{
Out.PredPointedVoxel.X = ActualCube_x_h; Out.PredPointedVoxel.Y = ActualCube_y_h; Out.PredPointedVoxel.Z = ActualCube_z_h;
Out.PointedVoxel.X = NewCube_x_h; Out.PointedVoxel.Y = NewCube_y_h; Out.PointedVoxel.Z = NewCube_z_h;
}
}
}
else
{
if( ( Collide_Z && !Collide_Y ) || ( Collide_Y && Collide_Z && Collision_s.w < Collision_v.w ) )
{
Out.PredPointedVoxel.X = ActualCube_x_s; Out.PredPointedVoxel.Y = ActualCube_y_s; Out.PredPointedVoxel.Z = ActualCube_z_s;
Out.PointedVoxel.X = NewCube_x_s; Out.PointedVoxel.Y = NewCube_y_s; Out.PointedVoxel.Z = NewCube_z_s;
}
else if( Collide_Y )
{
Out.PredPointedVoxel.X = ActualCube_x_v; Out.PredPointedVoxel.Y = ActualCube_y_v; Out.PredPointedVoxel.Z = ActualCube_z_v;
Out.PointedVoxel.X = NewCube_x_v; Out.PointedVoxel.Y = NewCube_y_v; Out.PointedVoxel.Z = NewCube_z_v;
}
}
}
Out.Collided = false;
Out.CollisionAxe = 0;
Out.CollisionFace = 0;
Out.CollisionDistance = 0.0f;
Out.CollisionPoint = btVector3.Zero;
return ( false );
//printf("first_h_x : %lf first_h_y %lf\n",first_h_x,first_h_y);
}
bool RayCast( ref RayCast_in In, ref RayCast_out Out )
{
btVector3 Delta_h, Delta_v, Delta_s;
btVector3 Offset_h = btVector3.Zero, Offset_v = btVector3.Zero, Offset_s = btVector3.Zero;
btVector3 Norm_h = btVector3.Zero, Norm_v = btVector3.Zero, Norm_s = btVector3.Zero;
btVector3 Collision_h = btVector3.Zero
, Collision_v = btVector3.Zero
, Collision_s = btVector3.Zero;
/*
Offset_h.X = Offset_h.Y = Offset_h.Z = Offset_h.w = 0.0;
Offset_v.X = Offset_v.Y = Offset_v.Z = Offset_v.w = 0.0;
Offset_s.X = Offset_s.Y = Offset_s.Z = Offset_s.w = 0.0;
Collision_h.X = Collision_h.Y = Collision_h.Z = Collision_h.w = 0.0;
Collision_v.X = Collision_v.Y = Collision_v.Z = Collision_v.w = 0.0;
Collision_s.X = Collision_s.Y = Collision_s.Z = Collision_s.w = 0.0;
*/
int ActualCube_x, ActualCube_y, ActualCube_z;
int NewCube_x, NewCube_y, NewCube_z;
bool Collide_X, Collide_Y, Collide_Z;
int i;
btVector3 Norm;
btVector3 Tmp;
Norm.x = 0; Norm.y = 0; Norm.z = 1; Norm.w = 0;
In.Camera.location.Apply( ref Norm, out Tmp );
// X axis rotation
//Tmp.Y = Norm.Y * cos(-In.Camera.Pitch/57.295779513) - Norm.Z * sin(-In.Camera.Pitch/57.295779513);
//Tmp.Z = Norm.Y * sin(-In.Camera.Pitch/57.295779513) + Norm.Z * cos(-In.Camera.Pitch/57.295779513);
//Norm.Y = Tmp.Y; Norm.Z = Tmp.Z;
// Y axis rotation
//Tmp.X = Norm.Z*sin(In.Camera.Yaw/57.295779513) + Norm.X * cos(In.Camera.Yaw/57.295779513);
//Tmp.Z = Norm.Z*cos(In.Camera.Yaw/57.295779513) - Norm.X * sin(In.Camera.Yaw/57.295779513);
//Norm.X = Tmp.X; Norm.Z = Tmp.Z;
/*
// Z axis rotation
Tmp.X = Norm.X * cos(roll/57.295779513) - Norm.Y * sin(roll/57.295779513);
Tmp.Y = Norm.X * sin(roll/57.295779513) + Norm.Y * cos(roll/57.295779513);
Norm.X = Tmp.X; Norm.Y = Tmp.Y;
*/
// Delta_h.X = tan((- yaw)/57.295779513);
Collide_X = Collide_Y = Collide_Z = false;
if( In.Camera.location.m_basis.m_el0.x >= 0.01 )
{
Collide_X = true;
Delta_h.y = Norm.y / -Norm.x;
Delta_h.z = Norm.z / -Norm.x;
Delta_h.w = 1.0f / Norm.x;
Collision_h.x = (float)( Math.Floor( In.Camera.location.m_origin.x / VoxelBlockSize ) + 1.0f ) * VoxelBlockSize;
Collision_h.y = ( Collision_h.x - In.Camera.location.m_origin.x ) * Delta_h.y + In.Camera.location.m_origin.y;
Collision_h.z = ( Collision_h.x - In.Camera.location.m_origin.x ) * Delta_h.z + In.Camera.location.m_origin.z;
Collision_h.w = ( Collision_h.x - In.Camera.location.m_origin.x ) * Delta_h.w;
Offset_h.x = VoxelBlockSize;
Offset_h.y = Delta_h.y * VoxelBlockSize;
Offset_h.z = Delta_h.z * VoxelBlockSize;
Offset_h.w = Delta_h.w * VoxelBlockSize;
Norm_h.x = Offset_h.x / ( VoxelBlockSize / 2 );
Norm_h.y = Offset_h.y / ( VoxelBlockSize / 2 );
Norm_h.z = Offset_h.z / ( VoxelBlockSize / 2 );
}
else if( In.Camera.location.m_basis.m_el0.x <= -0.01 )
{
Collide_X = true;
Delta_h.y = Norm.y / Norm.y;
Delta_h.z = Norm.z / Norm.z;
Delta_h.w = 1.0f / Math.Abs( Norm.x );
Collision_h.x = ( (float)Math.Floor( In.Camera.location.m_origin.x / VoxelBlockSize ) ) * VoxelBlockSize;
Collision_h.y = ( In.Camera.location.m_origin.x - Collision_h.x ) * Delta_h.y + In.Camera.location.m_origin.y;
Collision_h.z = ( In.Camera.location.m_origin.x - Collision_h.x ) * Delta_h.z + In.Camera.location.m_origin.z;
Collision_h.w = ( In.Camera.location.m_origin.x - Collision_h.x ) * Delta_h.w;
Offset_h.x = -VoxelBlockSize;
Offset_h.y = Delta_h.y * VoxelBlockSize;
Offset_h.z = Delta_h.z * VoxelBlockSize;
Offset_h.w = Delta_h.w * VoxelBlockSize;
Norm_h.x = Offset_h.x / ( VoxelBlockSize / 2 );
Norm_h.y = Offset_h.y / ( VoxelBlockSize / 2 );
Norm_h.z = Offset_h.z / ( VoxelBlockSize / 2 );
}
if( In.Camera.location.m_basis.m_el1.y >= 0.01 )
{
Collide_Y = true;
Delta_v.x = Norm.x / Norm.y;
Delta_v.z = Norm.z / -Norm.y;
Delta_v.w = 1 / Norm.y;
Collision_v.y = ( (float)Math.Floor( In.Camera.location.m_origin.y / VoxelBlockSize ) ) * VoxelBlockSize;
Collision_v.x = ( In.Camera.location.m_origin.y - Collision_v.y ) * Delta_v.x + In.Camera.location.m_origin.x;
Collision_v.z = ( In.Camera.location.m_origin.y - Collision_v.y ) * Delta_v.z + In.Camera.location.m_origin.z;
Collision_v.w = ( In.Camera.location.m_origin.y - Collision_v.y ) * Delta_v.w;
Offset_v.y = -VoxelBlockSize;
Offset_v.x = Delta_v.x * VoxelBlockSize;
Offset_v.z = Delta_v.z * VoxelBlockSize;
Offset_v.w = Delta_v.w * VoxelBlockSize;
Norm_v.x = Offset_v.x / ( VoxelBlockSize / 2 );
Norm_v.y = Offset_v.y / ( VoxelBlockSize / 2 );
Norm_v.z = Offset_v.z / ( VoxelBlockSize / 2 );
}
else if( In.Camera.location.m_basis.m_el1.y <= -0.01 )
{
Collide_Y = true;
Delta_v.x = Norm.x / -Norm.y;
Delta_v.z = Norm.z / +Norm.y;
Delta_v.w = 1.0f / -Norm.y;
Collision_v.y = ( (float)Math.Floor( In.Camera.location.m_origin.y / VoxelBlockSize ) + 1 ) * VoxelBlockSize;
Collision_v.x = ( Collision_v.y - In.Camera.location.m_origin.y ) * Delta_v.x + In.Camera.location.m_origin.x;
Collision_v.z = ( Collision_v.y - In.Camera.location.m_origin.y ) * Delta_v.z + In.Camera.location.m_origin.z;
Collision_v.w = ( Collision_v.y - In.Camera.location.m_origin.y ) * Delta_v.w;
Offset_v.y = VoxelBlockSize;
Offset_v.x = Delta_v.x * VoxelBlockSize;
Offset_v.z = Delta_v.z * VoxelBlockSize;
Offset_v.w = Delta_v.w * VoxelBlockSize;
Norm_v.x = Offset_v.x / ( VoxelBlockSize / 2 );
Norm_v.y = Offset_v.y / ( VoxelBlockSize / 2 );
Norm_v.z = Offset_v.z / ( VoxelBlockSize / 2 );
}
if( In.Camera.location.m_basis.m_el2.z >= 0.01 )
{
Collide_Z = true;
Delta_s.x = Norm.x / -Norm.z;
Delta_s.y = Norm.y / Norm.z;
Delta_s.w = 1.0f / -Norm.z;
Collision_s.z = ( (float)Math.Floor( In.Camera.location.m_origin.z / VoxelBlockSize ) + 1.0f ) * VoxelBlockSize;
Collision_s.x = ( Collision_s.z - In.Camera.location.m_origin.z ) * Delta_s.x + In.Camera.location.m_origin.x;
Collision_s.y = ( Collision_s.z - In.Camera.location.m_origin.z ) * Delta_s.y + In.Camera.location.m_origin.y;
Collision_s.w = ( Collision_s.z - In.Camera.location.m_origin.z ) * Delta_s.w;
Offset_s.z = VoxelBlockSize;
Offset_s.x = Delta_s.x * VoxelBlockSize;
Offset_s.y = Delta_s.y * VoxelBlockSize;
Offset_s.w = Delta_s.w * VoxelBlockSize;
Norm_s.x = Offset_s.x / ( VoxelBlockSize / 2 );
Norm_s.y = Offset_s.y / ( VoxelBlockSize / 2 );
Norm_s.z = Offset_s.z / ( VoxelBlockSize / 2 );
}
else if( In.Camera.location.m_basis.m_el2.z <= -0.01 )
{
Collide_Z = true;
Delta_s.x = Norm.x / +Norm.z;
Delta_s.y = Norm.y / -Norm.z;
Delta_s.w = 1.0f / Norm.z;
Collision_s.z = ( (float)Math.Floor( In.Camera.location.m_origin.z / VoxelBlockSize ) ) * VoxelBlockSize;
Collision_s.x = ( In.Camera.location.m_origin.z - Collision_s.z ) * Delta_s.x + In.Camera.location.m_origin.x;
Collision_s.y = ( In.Camera.location.m_origin.z - Collision_s.z ) * Delta_s.y + In.Camera.location.m_origin.y;
Collision_s.w = ( In.Camera.location.m_origin.z - Collision_s.z ) * Delta_s.w;
Offset_s.z = -VoxelBlockSize;
Offset_s.x = Delta_s.x * VoxelBlockSize;
Offset_s.y = Delta_s.y * VoxelBlockSize;
Offset_s.w = Delta_s.w * VoxelBlockSize;
Norm_s.x = Offset_s.x / ( VoxelBlockSize / 2 );
Norm_s.y = Offset_s.y / ( VoxelBlockSize / 2 );
Norm_s.z = Offset_s.z / ( VoxelBlockSize / 2 );
}
// printf("yaw: %04lf pitch: %lf Offset_y:%lf Offset_z:%lf xyz:%lf %lf %lf NXYZ:%lf %lf %lf Dxyz:%lf %lf %lf", yaw,pitch, Delta_h.Y, Delta_h.Z,x,y,Z, Norm_h.X, Norm_h.Y, Norm_h.Z, Delta_h.X, Delta_h.Y, Delta_h.Z);
//printf("Angle (y:%lf p:%lf) XYZ:(%lf %lf %lf) Off(%lf %lf %lf %lf) Coll(%lf %lf %lf %lf) Norm(%lg %lg %lf) :\n", yaw,pitch,x,y,Z, Offset_s.X, Offset_s.Y, Offset_s.Z, Offset_s.w, Collision_s.X, Collision_s.Y, Collision_s.Z, Collision_s.w, Norm_s.X,Norm_s.Y, Norm_s.Z);
int Match_h = 0;
int Match_s = 0;
int Match_v = 0;
int Cycle = 1;
float MinW = 1000000.0f;
for( i = 0; i < 150; i++ )
{
// Horizontal X axis.
if( Collide_X )
{
if( Match_h == 0 && Collision_h.w < MinW )
{
ActualCube_x = (int)Math.Floor( ( Collision_h.x - Norm_h.x ) / VoxelBlockSize ); ActualCube_y = (int)Math.Floor( ( Collision_h.y - Norm_h.y ) / VoxelBlockSize ); ActualCube_z = (int)Math.Floor( ( Collision_h.z - Norm_h.z ) / VoxelBlockSize );
NewCube_x = (int)Math.Floor( ( Collision_h.x + Norm_h.x ) / VoxelBlockSize ); NewCube_y = (int)Math.Floor( ( Collision_h.y + Norm_h.y ) / VoxelBlockSize ); NewCube_z = (int)Math.Floor( ( Collision_h.z + Norm_h.z ) / VoxelBlockSize );
if( GetVoxel( NewCube_x, NewCube_y, NewCube_z ) > 0 )
{
Out.PredPointedVoxel.X = ActualCube_x; Out.PredPointedVoxel.Y = ActualCube_y; Out.PredPointedVoxel.Z = ActualCube_z;
Out.PointedVoxel.X = NewCube_x; Out.PointedVoxel.Y = NewCube_y; Out.PointedVoxel.Z = NewCube_z;
// printf(" MATCH_H: %lf\n",Collision_h.w);
Match_h = Cycle;
MinW = Collision_h.w;
}
}
}
// Horizontal z axis.
if( Collide_Z )
{
if( Match_s == 0 && Collision_s.w < MinW )
{
ActualCube_x = (int)Math.Floor( ( Collision_s.x - Norm_s.x ) / VoxelBlockSize ); ActualCube_y = (int)Math.Floor( ( Collision_s.y - Norm_s.y ) / VoxelBlockSize ); ActualCube_z = (int)Math.Floor( ( Collision_s.z - Norm_s.z ) / VoxelBlockSize );
NewCube_x = (int)Math.Floor( ( Collision_s.x + Norm_s.x ) / VoxelBlockSize ); NewCube_y = (int)Math.Floor( ( Collision_s.y + Norm_s.y ) / VoxelBlockSize ); NewCube_z = (int)Math.Floor( ( Collision_s.z + Norm_s.z ) / VoxelBlockSize );
if( GetVoxel( NewCube_x, NewCube_y, NewCube_z ) > 0 )
{
Out.PredPointedVoxel.X = ActualCube_x; Out.PredPointedVoxel.Y = ActualCube_y; Out.PredPointedVoxel.Z = ActualCube_z;
Out.PointedVoxel.X = NewCube_x; Out.PointedVoxel.Y = NewCube_y; Out.PointedVoxel.Z = NewCube_z;
// printf(" MATCH_S: %lf\n",Collision_s.w);
Match_s = Cycle;
MinW = Collision_s.w;
}
}
}
// Vertical y axis.
if( Collide_Y )
{
if( Match_v == 0 && Collision_v.w < MinW )
{
ActualCube_x = (int)Math.Floor( ( Collision_v.x - Norm_v.x ) / VoxelBlockSize ); ActualCube_y = (int)Math.Floor( ( Collision_v.y - Norm_v.y ) / VoxelBlockSize ); ActualCube_z = (int)Math.Floor( ( Collision_v.z - Norm_v.z ) / VoxelBlockSize );
NewCube_x = (int)Math.Floor( ( Collision_v.x + Norm_v.x ) / VoxelBlockSize ); NewCube_y = (int)Math.Floor( ( Collision_v.y + Norm_v.y ) / VoxelBlockSize ); NewCube_z = (int)Math.Floor( ( Collision_v.z + Norm_v.z ) / VoxelBlockSize );
if( GetVoxel( NewCube_x, NewCube_y, NewCube_z ) > 0 )
{
Out.PredPointedVoxel.X = ActualCube_x; Out.PredPointedVoxel.Y = ActualCube_y; Out.PredPointedVoxel.Z = ActualCube_z;
Out.PointedVoxel.X = NewCube_x; Out.PointedVoxel.Y = NewCube_y; Out.PointedVoxel.Z = NewCube_z;
// printf(" MATCH_V: %lf\n",Collision_v.w);
Match_v = Cycle;
MinW = Collision_v.w;
}
}
}
//printf(" Match (H:%lf S:%lf V:%lf) \n", Collision_h.w, Collision_s.w, Collision_v.w);
if( Match_h > 0 && ( Match_h - Cycle ) < -100 ) return ( true );
if( Match_s > 0 && ( Match_s - Cycle ) < -100 ) return ( true );
if( Match_v > 0 && ( Match_v - Cycle ) < -100 ) return ( true );
Collision_h.x += Offset_h.x; Collision_h.y += Offset_h.y; Collision_h.z += Offset_h.z; Collision_h.w += Offset_h.w;
Collision_v.x += Offset_v.x; Collision_v.y += Offset_v.y; Collision_v.z += Offset_v.z; Collision_v.w += Offset_v.w;
Collision_s.x += Offset_s.x; Collision_s.y += Offset_s.y; Collision_s.z += Offset_s.z; Collision_s.w += Offset_s.w;
Cycle++;
}
// printf("\n");
return ( false );
//printf("first_h_x : %lf first_h_y %lf\n",first_h_x,first_h_y);
}
