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); }