3d translation normal, 3d collision normal, 3d reflection normal

;3d translation normal, 3d collision normal, 3d reflection normal by RemiD 20231101-1415
;this shows how to calculate and debug the translation normal, collision normal, reflection normal of a moving entity, after a collision with a surface

;graphics window
Global gwidth% = 854 : Global gheight% = 480
Graphics3D( gwidth, gheight, 32, 2 )

HidePointer()

SeedRnd( FixMilli() )

;input
Global MXDiff% : Global MYDiff%

;origine
Global origine = CreateCube()
ScaleMesh( origine,0.01,0.01,0.01 )
EntityColor( origine,255,000,255 )
EntityFX( origine,1 )

;camera
Global camera = CreateCamera()
CameraViewport( camera,0,0,GraphicsWidth(),GraphicsHeight() )
CameraRange( camera,1, 1000 )
CameraClsColor( camera,000,000,000 )

;ghost
Global ghost_root = CreatePivot()
Global ghost_rootyaw#
Global ghost_eyes = CreatePivot()
EntityParent( ghost_eyes, ghost_root, True )
Global ghost_eyespitch#

;room
Global room_mesh = CreateCube() : FlipMesh( room_mesh ) : ScaleMesh( room_mesh, 15.0/2, 15.0/2, 15.0/2 ) : PositionMesh( room_mesh, 0, 15.0/2, 0 )
EntityColor( room_mesh, 120, 120, 120 )
PositionEntity( room_mesh, +7.5, 0, +7.5, True )
EntityPickMode( room_mesh, 2 )

Xnormal = CreateCube() : ScaleMesh( Xnormal, 0.03, 0.03, 0.6/2 ) : PositionMesh( Xnormal, 0, 0, +0.6/2 )
HideEntity( Xnormal )

;spheres
Global spherescount%
Type Tsphere
 Field root
 Field renderer
 Field speed#
 Field tx#, ty#, tz#
 Field tnormal
 Field cnormal
 Field rnormal
End Type

For n% = 1 To 10
 spherescount = spherescount + 1
 sph.Tsphere = New Tsphere
 sph\root = CreatePivot()
 sph\renderer = CreateSphere(16) : ScaleMesh( sph\renderer, 1.0/2, 1.0/2, 1.0/2 ) 
 EntityColor( sph\renderer, Rand(025,250), Rand(025,250), Rand(025,250) )
 ;EntityParent( sph\renderer, sph\root, True )
 PositionEntity( sph\root, Rnd(0+0.5+1,15.0-0.5-1), Rnd(0+0.5+1,15.0-0.5-1), Rnd(0+0.5+1,15.0-0.5-1), True )
 PositionEntity( sph\renderer, EntityX(sph\root,True), EntityY(sph\root,True), EntityZ(sph\root,True), True )
 sph\speed = Rnd( 0.1, 0.9 ) 
 ;new random vector
 vx# = Rnd(-1,+1) : vy# = Rnd(-1,+1) : vz# = Rnd(-1,+1)
 ;normal vector
 vlength# = Sqr( ( vx * vx ) + ( vy * vy ) + ( vz  * vz ) )
 nx# = vx / vlength : ny# = vy / vlength : nz# = vz / vlength
 ;translation vector
 sph\tx = nx * sph\speed : sph\ty = ny * sph\speed : sph\tz = nz * sph\speed
 EntityPickMode( sph\renderer, 2 )
 ;debug translation normal
 sph\tnormal = CopyEntity( Xnormal )
 EntityFX( sph\tnormal, 1 ) : EntityColor( sph\tnormal, 125, 125, 250 ) ;blue
 ;debug collision normal
 sph\cnormal = CopyEntity( Xnormal )
 EntityFX( sph\cnormal, 1 ) : EntityColor( sph\cnormal, 250, 000, 250 ) ;magenta
 ;debug reflection normal
 sph\rnormal = CopyEntity( Xnormal )
 EntityFX( sph\rnormal, 1 ) : EntityColor( sph\rnormal, 000, 250, 250 ) ;cyan
Next

Global dlight = CreateLight(1)
LightColor( dlight, 240, 240, 240 )
PositionEntity( dlight, -1000, +1000, -1000, True )
RotateEntity( dlight, +45, 0, 0, True )
AmbientLight( 060, 060, 060 )

PositionEntity( ghost_root, +7.5, +7.5, -15, True )

Global MainTimer = CreateTimer(30)

Main()

End()

Function Main()

 Repeat

  MainMilliStart% = FixMilli()

  MXDiff = MouseXSpeed() : MYDiff = MouseYSpeed()

  UpdateGhost()

  UpdateSpheres()

  WireFrame(False) 
  If( KeyDown(2)=1 )
   WireFrame(True)
  EndIf

  PositionEntity( camera, EntityX(ghost_eyes,True), EntityY(ghost_eyes,True), EntityZ(ghost_eyes,True), True )
  RotateEntity( camera, EntityPitch(ghost_eyes,True), EntityYaw(ghost_eyes,True), EntityRoll(ghost_eyes,True), True )

  CameraViewport(camera,0,0,GraphicsWidth(),GraphicsHeight())
  CameraClsColor(camera,000,000,000)
  SetBuffer(BackBuffer())
  RenderWorld()

  Color(255,255,255)
  CText( "Tris = "+TrisRendered(), 0, 0 ) 
  CText( "FPS = "+FPS, 0, 15 )

  ;Flip(1)
  WaitTimer(MainTimer)
  VWait():Flip(False)

  MainMilliTime = FixMilli() - MainMilliStart
  If( MainMilliTime < 1 )
   MainMilliTime = 1
  EndIf

  FPS% = 1000.0/MainMilliTime 

 Until( KeyDown(1)=1 )

End Function

Function FixMilli%()

 Return MilliSecs() And $7FFFFFFF

End Function

Function CText(TextStr$,PX%,PY%)

 Text(PX,PY,TextStr,False,False)

End Function

Function Distance2D#(PAX#,PAZ#,PBX#,PBZ#)

 Distance2D# = Sqr( ( ( PBX - PAX ) * ( PBX - PAX ) ) + ( ( PBZ - PAZ ) * ( PBZ - PAZ ) ) )
 Return Distance2D

End Function

Function Distance3D#(PAX#,PAY#,PAZ#,PBX#,PBY#,PBZ#)

 Distance3D# = Sqr( ( ( PBX - PAX ) * ( PBX - PAX ) ) + ( ( PBY - PAY ) * ( PBY - PAY ) ) + ( ( PBZ - PAZ ) * ( PBZ - PAZ ) ) )
 Return Distance3D

End Function

Function UpdateGhost()

 MoveMouse(GraphicsWidth()/2,GraphicsHeight()/2)
 Ghost_EyesPitch = Ghost_EyesPitch + Float(MYDiff)/10
 If( Ghost_EyesPitch < -89 )
  Ghost_EyesPitch = -89
 Else If( Ghost_EyesPitch > 89 )
  Ghost_EyesPitch = 89
 EndIf
 RotateEntity(Ghost_Eyes,Ghost_EyesPitch,0,0,False)
 Ghost_RootYaw = Ghost_RootYaw - Float(MXDiff)/10
 RotateEntity(Ghost_Root,0,Ghost_RootYaw,0,False)

 If( KeyDown(42) = 0 And KeyDown(29) = 0 )
  Speed# = 0.1
 Else If( KeyDown(42) = 1 And KeyDown(29) = 0 )
  Speed# = 1
 Else If( KeyDown(42) = 0 And KeyDown(29) = 1 )
  Speed# = 0.01
 EndIf

 If( KeyDown(17)=1 )
  MoveEntity(Ghost_Root,0,0,Speed)
 Else If( KeyDown(31)=1 )
  MoveEntity(Ghost_Root,0,0,-Speed)
 EndIf
 If( KeyDown(30)=1 )
  MoveEntity(Ghost_Root,-Speed,0,0)
 Else If( KeyDown(32)=1 )
  MoveEntity(Ghost_Root,Speed,0,0)
 EndIf
 If( KeyDown(16)=1 )
  MoveEntity(Ghost_Root,0,-Speed,0)
 Else If( KeyDown(18)=1 )
  MoveEntity(Ghost_Root,0,Speed,0)
 EndIf

End Function

Function UpdateSpheres()

  If( KeyHit(57)=1 )
   For sph.Tsphere = Each Tsphere
   ;new speed
   sph\speed = Rnd( 0.1, 0.9 ) 
   ;new random vector
    vx# = Rnd(-1,+1) : vy# = Rnd(-1,+1) : vz# = Rnd(-1,+1)
    ;normal vector
    vlength# = Sqr( ( vx * vx ) + ( vy * vy ) + ( vz  * vz ) )
    nx# = vx / vlength : ny# = vy / vlength : nz# = vz / vlength
    ;translation vector
    sph\tx = nx * sph\speed : sph\ty = ny * sph\speed : sph\tz = nz * sph\speed
   Next
  EndIf

 For sph.Tsphere = Each Tsphere
  ;before translating the entity, check if it will collide with an obstacle
  willcollidewithobstacle% = False
  HideEntity( sph\renderer )
  TFormVector( sph\tx*1.1, sph\ty*1.1, sph\tz*1.1, sph\renderer, 0 )
  pickedent% = LinePick( EntityX(sph\renderer,True), EntityY(sph\renderer,True), EntityZ(sph\renderer,True), TFormedX(), TFormedY(), TFormedZ(), 0.5 )
  If( pickedent <> 0 )

   ;collision normal
   PositionEntity( sph\cnormal, PickedX(), PickedY(), PickedZ(), True )
   AlignToVector( sph\cnormal, PickedNX(), PickedNY(), PickedNZ(), 3, 1.0 )

   ;if collision, calculate new entity position
   ;distance between entity and collision point
   d# = Distance3d( EntityX(sph\renderer,True), EntityY(sph\renderer,True), EntityZ(sph\renderer,True), PickedX(), PickedY(), PickedZ() )
   ;entity normal vector
   vx# = sph\tx : vy# = sph\ty : vz# = sph\tz
   vlength# = Sqr( ( vx * vx ) + ( vy * vy ) + ( vz  * vz ) )
   nx# = vx / vlength : ny# = vy / vlength : nz# = vz / vlength

   ;translation normal
   PositionEntity( sph\tnormal, PickedX(), PickedY(), PickedZ(), True )
   AlignToVector( sph\tnormal, nx, ny, nz, 3, 1.0 )
   MoveEntity( sph\tnormal, 0, 0, -0.6 )

   ;new position = entity position + entity normal vector * ( distance - (radius*1.1) )   
   new_x# = EntityX(sph\renderer,True)+nx*(d-(0.5*1.1)) : new_y# = EntityY(sph\renderer,True)+ny*(d-(0.5*1.1)) : new_z# = EntityZ(sph\renderer,True)+nz*(d-(0.5*1.1)) 
   willcollidewithobstacle = True
  EndIf
  ShowEntity( sph\renderer ) 

  If( willcollidewithobstacle = True )

   ;reposition entity
   PositionEntity( sph\renderer, new_x, new_y, new_z, True )

   ;sph\tx = 0 : sph\ty = 0 : sph\tz = 0

   ;calculate reflection normal vector
   curnx# = nx : curny# = ny : curnz# = nz
   collnx# = PickedNX() : collny# = PickedNY() : collnz# = PickedNZ()
   dot# = curnx*collnx + curny*collny + curnz*collnz
   reflvx# = curnx - 2.0 * collnx * dot : reflvy# = curny - 2.0 * collny * dot : reflvz# = curnz - 2.0 * collnz * dot
   reflvlength# = Sqr( ( reflvx * reflvx ) + ( reflvy * reflvy ) + ( reflvz * reflvz ) )
   reflnx# = reflvx / reflvlength : reflny# = reflvy / reflvlength : reflnz# = reflvz / reflvlength

   ;reflection normal
   PositionEntity( sph\rnormal, PickedX(), PickedY(), PickedZ(), True )
   AlignToVector( sph\rnormal, reflnx, reflny, reflnz, 3, 1.0 )

   ;new translation vector
   sph\tx = reflnx*sph\speed*1.0 : sph\ty = reflny*sph\speed*1.0 : sph\tz = reflnz*sph\speed*1.0

  Else If( willcollidewithobstacle = False )

   ;translate sphere
   TranslateEntity( sph\renderer, sph\tx, sph\ty, sph\tz )

  EndIf

 Next

End Function