FreeBASIC ビオ・サバールの法則

'Physics example: Biot-Savart law
'by badidea ≫ Dec 29, 2011 17:05 
'https://www.freebasic.net/forum/viewtopic.php?f=7&t=19023

'      z
'      |
'      |
'      |
'      +-------- y
'     /
'    /
'   x

'#include "betterGraph_02.bi"

' Sets the graphics method GDI
' 描画方法を GDI に設定
SetEnviron("fbgfx=GDI")

const as double pi = 3.141592654
const as double u0 = 4 * pi * 1e-7 '[V・s/(A・m)] or [T・m/A]
const as double u0div4pi = 1e-7

const as integer xmin_3d_color = -100
const as integer xmax_3d_color = +100

const as integer scrn_w = 1024
const as integer scrn_h = 768
const as double ppm = 200 'pixels per meter
const as integer coil_offset_x = -280
const as integer coil_offset_y = -160

'-------------------- GRAPH -----------------------

type xy_dbl
  x as double
  y as double
end type

type xy_int
  x as integer
  y as integer
end type

type graph_type
  onScreenOffset as xy_dbl
  onScreenSize as xy_dbl
  dotColour as integer
  axisColour as integer
  gridColour as integer
  physicalMin as xy_dbl
  physicalMax as xy_dbl
  physicalMark as xy_dbl
  'private variables:
  scaling as xy_dbl
  screenSize as xy_int
  'procedures:
  declare sub init()
  declare sub setAxis()
  declare sub setGrid()
  declare sub setDot(physX as double, physY as double)
  declare sub setDotCustom(physX as double, physY as double, c as integer)
  declare sub setCircle(physX as double, physY as double)
  declare sub setLine(physX1 as double, physY1 as double, physX2 as double, physY2 as double)
end type

sub graph_type.init()
  'Pixels per physical units
  scaling.x = onScreenSize.x / (physicalMax.x - physicalMin.x)
  scaling.y = onScreenSize.y / (physicalMax.y - physicalMin.y)
  screeninfo screenSize.x, screenSize.y
end sub

sub graph_type.setAxis()
  dim as integer x0 = onScreenOffset.x
  dim as integer y0 = (screenSize.y - 1) - onScreenOffset.y
  line (x0, y0)-step(0, -onScreenSize.y), axisColour
  line (x0, y0)-step(onScreenSize.x, 0), axisColour
  draw string (x0, y0), str(physicalMin.x), axisColour
  draw string (x0, y0 - 20), str(physicalMin.y), axisColour
  draw string (x0, y0 - onScreenSize.y), str(physicalMax.y), axisColour
  draw string (x0 + onScreenSize.x, y0), str(physicalMax.x), axisColour
end sub

sub graph_type.setGrid()
  dim as integer x0 = onScreenOffset.x
  dim as integer y0 = (screenSize.y - 1) - onScreenOffset.y
  dim as integer x1 = x0 + onScreenSize.x
  dim as integer y1 = y0 - onScreenSize.y
  dim as integer x, y
  for y = y1 to y0 step onScreenSize.y \ 10
    line(x0, y)-(x1,y), gridColour
  next
  for x = x0 to x1 step onScreenSize.x \ 10
    line(x, y0)-(x,y1), gridColour
  next
end sub

sub graph_type.setDot(physX as double, physY as double)
  dim as integer x, y
  x = (physX - physicalMin.x) * scaling.x + onScreenOffset.x
  y = (screenSize.y - 1) - ((physY - physicalMin.y) * scaling.y + onScreenOffset.y)
  pset (x, y), dotColour
end sub

sub graph_type.setDotCustom(physX as double, physY as double, c as integer)
  dim as integer x, y
  x = (physX - physicalMin.x) * scaling.x + onScreenOffset.x
  y = (screenSize.y - 1) - ((physY - physicalMin.y) * scaling.y + onScreenOffset.y)
  pset (x, y), c
end sub

sub graph_type.setCircle(physX as double, physY as double)
  dim as integer x, y
  x = (physX - physicalMin.x) * scaling.x + onScreenOffset.x
  y = (screenSize.y - 1) - ((physY - physicalMin.y) * scaling.y + onScreenOffset.y)
  circle (x, y), 3, dotColour
end sub

sub graph_type.setLine(physX1 as double, physY1 as double, physX2 as double, physY2 as double)
  dim as integer x1, y1, x2, y2
  x1 = (physX1 - physicalMin.x) * scaling.x + onScreenOffset.x
  y1 = (screenSize.y - 1) - ((physY1 - physicalMin.y) * scaling.y + onScreenOffset.y)
  x2 = (physX2 - physicalMin.x) * scaling.x + onScreenOffset.x
  y2 = (screenSize.y - 1) - ((physY2 - physicalMin.y) * scaling.y + onScreenOffset.y)
  line (x1, y1)-(x2, y2), dotColour
end sub

'-------------------- PIXEL -----------------------

type rgba_type
   b as ubyte
   g as ubyte
   r as ubyte
   a as ubyte
end type

union pixel_type
   ch as rgba_type
   value as integer
end union

function createPixel(r as ubyte, g as ubyte, b as ubyte) as pixel_type
   dim as pixel_type pixel
   pixel.ch.r = r
   pixel.ch.g = g
   pixel.ch.b = b
   return pixel
end function

sub scalePixel(byref pixel as pixel_type, xDepth as double)
   dim as double factor = 0.1 + 0.9 * (xDepth - xmin_3d_color) / (xmax_3d_color - xmin_3d_color)
   if factor > 1 then factor = 1
   if factor < 0.1 then factor = 0.1
   pixel.ch.r *= factor
   pixel.ch.g *= factor
   pixel.ch.b *= factor
end sub

'-------------------- DRAWING -----------------------

type point2d
   as double x, y
end type

type point3d
   as double x, y, z
end type

function p3d2d(p3d as point3d) as point2d
   dim as point2d p2d
   p2d.x = (scrn_w shr 1) + (p3d.y - p3d.x / 4) * ppm + coil_offset_x
   p2d.y = (scrn_h shr 1) + (p3d.x / 2 - p3d.z) * ppm + coil_offset_y
   return p2d
end function

sub pset3d(p3d as point3d, pixel as pixel_type)
   dim as point2d p2d = p3d2d(p3d)
   scalePixel(pixel, p3d.x)
   pset (p2d.x, p2d.y), pixel.value
end sub

sub marker3d(p3d as point3d, pixel as pixel_type)
   dim as point2d p2d = p3d2d(p3d)
   circle (p2d.x, p2d.y), 3, pixel.value
end sub

sub line3d(p3d1 as point3d, p3d2 as point3d, pixel as pixel_type)
   dim as point3d p3dm
   dim as point2d p2d1 = p3d2d(p3d1)
   dim as point2d p2d2 = p3d2d(p3d2)
   p3dm.x = (p3d1.x + p3d2.x) \ 2
   p3dm.y = (p3d1.y + p3d2.y) \ 2
   p3dm.z = (p3d1.z + p3d2.z) \ 2
   scalePixel(pixel, p3dm.x)
   line(p2d1.x, p2d1.y)-(p2d2.x, p2d2.y), pixel.value
end sub

'-------------------- VECTOR -----------------------

function vector_substract(v1 as point3d, v2 as point3d) as point3d
   dim as point3d v
   v.x = v1.x - v2.x
   v.y = v1.y - v2.y
   v.z = v1.z - v2.z
   return v
end function

function vector_add(v1 as point3d, v2 as point3d) as point3d
   dim as point3d v
   v.x = v1.x + v2.x
   v.y = v1.y + v2.y
   v.z = v1.z + v2.z
   return v
end function

function vector_average(v1 as point3d, v2 as point3d) as point3d
   dim as point3d v
   v.x = (v1.x + v2.x) / 2
   v.y = (v1.y + v2.y) / 2
   v.z = (v1.z + v2.z) / 2
   return v
end function

function vector_multiply(v1 as point3d, mul as double) as point3d
   dim as point3d v
   v.x = v1.x * mul
   v.y = v1.y * mul
   v.z = v1.z * mul
   return v
end function

function vector_divide(v1 as point3d, div as double) as point3d
   dim as point3d v
   v.x = v1.x / div
   v.y = v1.y / div
   v.z = v1.z / div
   return v
end function

function vector_squared(v1 as point3d) as double
   return v1.x * v1.x + v1.y * v1.y + v1.z * v1.z
end function

function vector_magnitude(v1 as point3d) as double
   return sqr(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z)
end function

declare function vector_normalise (v1 as point3d) as point3d

function vector_normalise(v1 as point3d) as point3d
   dim as double magnitude = sqr(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z)
   dim as point3d v
   v.x = v1.x / magnitude
   v.y = v1.y / magnitude
   v.z = v1.z / magnitude
   return v
end function

function vector_cross_product(v1 as point3d, v2 as point3d) as point3d
   dim as point3d v
   v.x = v1.y * v2.z - v1.z * v2.y
   v.y = v1.z * v2.x - v1.x * v2.z
   v.z = v1.x * v2.y - v1.y * v2.x
   return v
end function

sub vector_print(v1 as point3d)
   print "  x:"; v1.x
   print "  y:"; v1.y
   print "  z:"; v1.z
   print "  magnitude:"; vector_magnitude(v1)
end sub

'-------------------------------------------------

sub rotate(p3d as point3d, xTheta as double, yTheta as double, zTheta as double)
   'From tutorials Relsoft
   dim as double x = p3d.x, y = p3d.y, z = p3d.z
   dim as double xNew, yNew, zNew
   '***Rotation on the Z-axis
   yNew = y*cos(xTheta) - z*sin(xTheta)
   zNew = z*cos(xTheta) + y*sin(xTheta)
   y = yNew
   z = zNew

   '***Rotation on the Y-axis
   zNew = z*cos(yTheta) - x*sin(yTheta)
   xNew = x*cos(yTheta) + z*sin(yTheta)
   x = xNew

   '***Rotation on the Z-axis
   xNew = x*cos(zTheta) - y*sin(zTheta)
   yNew = y*cos(zTheta) + x*sin(zTheta)

   p3d.x = xNew
   p3d.y = yNew
   p3d.z = zNew
end sub

sub translate(p3d as point3d, xTrans as double, yTrans as double, zTrans as double)
   p3d.x += xTrans
   p3d.y += yTrans
   p3d.z += zTrans
end sub

sub multiply(p3d as point3d, xMul as double, yMul as double, zMul as double)
   p3d.x *= xMul
   p3d.y *= yMul
   p3d.z *= zMul
end sub

'-------------------- COIL -----------------------

sub drawCoilSection(p1 as point3d, p2 as point3d)
   dim as point3d p3
   line3d(p1, p2, createPixel(255, 0, 0))
   p3 = vector_average(p1, p2)
   pset3d(p3, createPixel(255, 255, 255))
end sub

type coil_type
   as double current '+/- [A]
   as double radius '[m]
   as double xTrans, yTrans, zTrans '[m]
   as double xTheta, yTheta, zTheta '[rad]
   declare function getPoint (angle as double) as point3d
   declare function getBField(poi as point3d, numSections as integer) as point3d
   declare sub show(numSections as integer)
end type

function coil_type.getPoint (angle as double) as point3d
   dim as point3d p1 = type<point3d> (cos(angle), 0, sin(angle))
   multiply(p1, radius, radius, radius)
   rotate(p1, xTheta, yTheta, zTheta)
   translate(p1, xTrans, yTrans, zTrans)
   return p1
end function

function coil_type.getBField(poi as point3d, numSections as integer) as point3d
   dim as double angle, dAngle
   dim as point3d vector_dl 'short piece of current conductor [m]
   dim as point3d vector_r 'from conductior piece to point of interest
   dim as point3d vector_dB, vector_B '[T] or [V・s・m-2]
   dim as point3d p1, p2
   vector_B = type<point3d> (0, 0, 0)
   dAngle  = 2 * pi / numSections
   p2 = getPoint(-pi)
   'loop coil in steps, up to pi() and a bit too prvent gap
   for angle = -pi + dAngle to +pi + dAngle/10 step dAngle
      p1 = p2
      p2 = getPoint(angle)
      'drawCoilSection(p1, p2)
      vector_dl = vector_substract(p2, p1)
      vector_r = vector_substract(poi, p1)
      'vector_dB [T] = u0 [(V*s)/(A*m)] /  4*pi() * _
      '(I [A] * vector_dl [m] cross_product unit_vector_r []) / abs(r^2) [m2]
      vector_dB = _
      vector_multiply(vector_cross_product(vector_multiply(vector_dl, current),   _
      vector_normalise(vector_r)), u0div4pi / vector_squared(vector_r))
      vector_B = vector_add(vector_B, vector_dB)
   next
   return vector_B
end function

sub coil_type.show(numSections as integer)
   dim as double angle, dAngle
   dim as point3d p1, p2
   dAngle  = 2 * pi / numSections
   p2 = getPoint(-pi)
   'loop coil in steps, up to pi() and a bit too prvent gap
   for angle = -pi + dAngle to +pi + dAngle/10 step dAngle
      p1 = p2
      p2 = getPoint(angle)
      drawCoilSection(p1, p2)
   next
end sub

'------------------ PARTICLE ---------------------

type particle
   dim as point3d position
   dim as point3d velocity
   dim as point3d accelleration
   dim as point3d force
   dim as double mass
   dim as double charge
end type

'-------------------- MAIN -----------------------

dim shared as point3d p0 = type<point3d> (0, 0, 0)
dim as integer i, j
dim as point3d vector_B '[T] or [V・s・m-2]
dim as point3d poi 'point of interest
dim as double y, x

dim as graph_type gr1, gr2, gr3

screenres scrn_w, scrn_h, 32

with gr1
  .onScreenOffset.x = 500
  .onScreenOffset.y = 400 'from bottom of screen
  .onScreenSize.x = 450
  .onScreenSize.y = 350
  .dotColour = &hff00ff
  .axisColour = &hc0c0c0
  .gridColour = &h444444
  .physicalMin.x = -1.00 'm
  .physicalMax.x = +1.00 'm
  .physicalMin.y = -2.0 'T
  .physicalMax.y = +2.0 'T
  .init()
  .setGrid()
  .setAxis()
end with

gr2 = gr1

with gr2
  .onScreenOffset.y = 20 'from bottom of screen
  .physicalMin.y = -0.02 'T
  .physicalMax.y = +0.02 'T
  .init()
  .setGrid()
  .setAxis()
end with

gr3  = gr1

with gr3
  .onScreenOffset.x = 20
  .onScreenOffset.y = 20 'from bottom of screen
  .physicalMin.y = -2.0 'm
  .physicalMax.y = +2.0 'm
  .init()
  .setGrid()
  .setAxis()
end with

'Draw axis for coils
for i = -200 to +200
   pset3d(type<point3d> (i / ppm, 0, 0), createPixel(255, 255, 255))
   pset3d(type<point3d> (0, i / ppm, 0), createPixel(255, 255, 255))
   pset3d(type<point3d> (0, 0, i / ppm), createPixel(255, 255, 255))
   draw string (p3d2d(type<point3d>(1, 0, 0)).x, p3d2d(type<point3d>(1, 0, 0)).y), "x", &hd0d0d0
   draw string (p3d2d(type<point3d>(0, 1, 0)).x, p3d2d(type<point3d>(0, 1, 0)).y), "y", &hd0d0d0
   draw string (p3d2d(type<point3d>(0, 0, 1)).x, p3d2d(type<point3d>(0, 0, 1)).y), "z", &hd0d0d0
next

dim as coil_type coil(0 to 3)

for i = lbound(coil) to ubound(coil)
   coil(i).current = 22 * 20 * 1000 '[A]
   coil(i).radius = 0.5 '[m]
next

coil(0).yTrans = -0.40 '[m]
coil(1).yTrans = -0.20 '[m]
coil(2).yTrans = +0.20 '[m]
coil(3).yTrans = +0.40 '[m]

coil(3).xTrans = 0.002 '2 cm
coil(3).zTheta = 0.03 'rad

const as integer numCoilSections = 20

'show the coils
for i = lbound(coil) to ubound(coil)
   coil(i).show(numCoilSections)
next

'show the field
for y = gr1.physicalMin.x to gr1.physicalMax.x step 0.001
   poi = type<point3d> (x, y, 0) 'point of interest, shifted on x
   vector_B = type<point3d> (0, 0, 0)
   for i = lbound(coil) to ubound(coil)
      vector_B = vector_add(vector_B, coil(i).getBField(poi, numCoilSections))
   next

   pset3d(poi, createPixel(255, 255, 0))
   
   gr1.setDotCustom(y, vector_B.x, &h770077)
   gr1.setDotCustom(y, vector_B.y, &h007777)
   gr1.setDotCustom(y, vector_B.z, &h777700)
   
   gr2.setDotCustom(y, vector_B.x, &h770077)
   gr2.setDotCustom(y, vector_B.z, &h777700)

   draw string (gr1.onScreenOffset.x + gr1.onScreenSize.x - 30, scrn_h - (gr1.onScreenOffset.y + gr1.onScreenSize.y - 4)), "B.x", &hff00ff
   draw string (gr1.onScreenOffset.x + gr1.onScreenSize.x - 30, scrn_h - (gr1.onScreenOffset.y + gr1.onScreenSize.y - 14)), "B.y", &h00ffff
   draw string (gr1.onScreenOffset.x + gr1.onScreenSize.x - 30, scrn_h - (gr1.onScreenOffset.y + gr1.onScreenSize.y - 24)), "B.z", &hffff00
next

const as integer numTracers = 16
dim as point3d tracer(numTracers-1)

tracer(0) = type<point3d> (+0.4, 0, 0)
for j = 1 to numTracers-1
   tracer(j) = tracer(0)
   rotate(tracer(j), 0, (2 * pi * j) / numTracers, 0)
next

print "Press any key to stop simulation"
while inkey = ""
   for j = 0 to numTracers-1
      pset3d(tracer(j), createPixel(127, (j/numTracers)*255, 127))
      gr3.setDotCustom(tracer(j).y, tracer(j).x, &hffff00)
      gr3.setDotCustom(tracer(j).y, tracer(j).z, &hffffff)

      vector_B = type<point3d> (0, 0, 0)
      for i = lbound(coil) to ubound(coil)
         vector_B = vector_add(vector_B, coil(i).getBField(tracer(j), numCoilSections))
      next

      tracer(j) = vector_add(tracer(j), vector_multiply(vector_normalise(vector_B), 0.0005))
   next
'   sleep 1,1
wend

print
print "Simulation Ended."
sleep

'Physics example: Biot-Savart law
'by dodicat ≫ Dec 29, 2011 23:50 
'https://www.freebasic.net/forum/viewtopic.php?f=7&t=19023

'      z
'      |
'      |
'      |
'      +-------- y
'     /
'    /
'   x

'#include "betterGraph_02.bi"

' Sets the graphics method GDI
' 描画方法を GDI に設定
SetEnviron("fbgfx=GDI")

const as single pi = 3.141592654
const as single u0 = 4 * pi * 1e-7 '[V・s/(A・m)] or [T・m/A]
const as single u0div4pi = 1e-7

const as integer xmin_3d_color = -100
const as integer xmax_3d_color = +100

const as integer scrn_w = 1024
const as integer scrn_h = 768
const as single ppm = 200 'pixels per meter
const as integer coil_offset_x = -280
const as integer coil_offset_y = -160

'-------------------- GRAPH -----------------------

type xy_dbl
  x as double
  y as double
end type

type xy_int
  x as integer
  y as integer
end type

type graph_type
  onScreenOffset as xy_dbl
  onScreenSize as xy_dbl
  dotColour as integer
  axisColour as integer
  gridColour as integer
  physicalMin as xy_dbl
  physicalMax as xy_dbl
  physicalMark as xy_dbl
  'private variables:
  scaling as xy_dbl
  screenSize as xy_int
  'procedures:
  declare sub init()
  declare sub setAxis()
  declare sub setGrid()
  declare sub setDot(physX as double, physY as double)
  declare sub setDotCustom(physX as double, physY as double, c as integer)
  declare sub setCircle(physX as double, physY as double)
  declare sub setLine(physX1 as double, physY1 as double, physX2 as double, physY2 as double)
end type

sub graph_type.init()
  'Pixels per physical units
  scaling.x = onScreenSize.x / (physicalMax.x - physicalMin.x)
  scaling.y = onScreenSize.y / (physicalMax.y - physicalMin.y)
  screeninfo screenSize.x, screenSize.y
end sub

sub graph_type.setAxis()
  dim as integer x0 = onScreenOffset.x
  dim as integer y0 = (screenSize.y - 1) - onScreenOffset.y
  line (x0, y0)-step(0, -onScreenSize.y), axisColour
  line (x0, y0)-step(onScreenSize.x, 0), axisColour
  draw string (x0, y0), str(physicalMin.x), axisColour
  draw string (x0, y0 - 20), str(physicalMin.y), axisColour
  draw string (x0, y0 - onScreenSize.y), str(physicalMax.y), axisColour
  draw string (x0 + onScreenSize.x, y0), str(physicalMax.x), axisColour
end sub

sub graph_type.setGrid()
  dim as integer x0 = onScreenOffset.x
  dim as integer y0 = (screenSize.y - 1) - onScreenOffset.y
  dim as integer x1 = x0 + onScreenSize.x
  dim as integer y1 = y0 - onScreenSize.y
  dim as integer x, y
  for y = y1 to y0 step onScreenSize.y \ 10
    line(x0, y)-(x1,y), gridColour
  next
  for x = x0 to x1 step onScreenSize.x \ 10
    line(x, y0)-(x,y1), gridColour
  next
end sub

sub graph_type.setDot(physX as double, physY as double)
  dim as integer x, y
  x = (physX - physicalMin.x) * scaling.x + onScreenOffset.x
  y = (screenSize.y - 1) - ((physY - physicalMin.y) * scaling.y + onScreenOffset.y)
  pset (x, y), dotColour
end sub

sub graph_type.setDotCustom(physX as double, physY as double, c as integer)
  dim as integer x, y
  x = (physX - physicalMin.x) * scaling.x + onScreenOffset.x
  y = (screenSize.y - 1) - ((physY - physicalMin.y) * scaling.y + onScreenOffset.y)
  pset (x, y), c
end sub

sub graph_type.setCircle(physX as double, physY as double)
  dim as integer x, y
  x = (physX - physicalMin.x) * scaling.x + onScreenOffset.x
  y = (screenSize.y - 1) - ((physY - physicalMin.y) * scaling.y + onScreenOffset.y)
  circle (x, y), 3, dotColour
end sub

sub graph_type.setLine(physX1 as double, physY1 as double, physX2 as double, physY2 as double)
  dim as integer x1, y1, x2, y2
  x1 = (physX1 - physicalMin.x) * scaling.x + onScreenOffset.x
  y1 = (screenSize.y - 1) - ((physY1 - physicalMin.y) * scaling.y + onScreenOffset.y)
  x2 = (physX2 - physicalMin.x) * scaling.x + onScreenOffset.x
  y2 = (screenSize.y - 1) - ((physY2 - physicalMin.y) * scaling.y + onScreenOffset.y)
  line (x1, y1)-(x2, y2), dotColour
end sub

'-------------------- PIXEL -----------------------

type rgba_type
        b as ubyte
        g as ubyte
        r as ubyte
        a as ubyte
end type

union pixel_type
        ch as rgba_type
        value as integer
end union

function createPixel(r as ubyte, g as ubyte, b as ubyte) as pixel_type
        dim as pixel_type pixel
        pixel.ch.r = r
        pixel.ch.g = g
        pixel.ch.b = b
        return pixel
end function

sub scalePixel(byref pixel as pixel_type, xDepth as single)
        dim as single factor = 0.1 + 0.9 * (xDepth - xmin_3d_color) / (xmax_3d_color - xmin_3d_color)
        if factor > 1 then factor = 1
        if factor < 0.1 then factor = 0.1
        pixel.ch.r *= factor
        pixel.ch.g *= factor
        pixel.ch.b *= factor
end sub

'-------------------- DRAWING -----------------------

type point2d
        as single x, y
end type

type point3d
        as single x, y, z
end type

function p3d2d(p3d as point3d) as point2d
        dim as point2d p2d
        p2d.x = (scrn_w shr 1) + (p3d.y - p3d.x / 4) * ppm + coil_offset_x
        p2d.y = (scrn_h shr 1) + (p3d.x / 2 - p3d.z) * ppm + coil_offset_y
        return p2d
end function

sub pset3d(p3d as point3d, pixel as pixel_type)
        dim as point2d p2d = p3d2d(p3d)
        scalePixel(pixel, p3d.x)
        pset (p2d.x, p2d.y), pixel.value
end sub

sub marker3d(p3d as point3d, pixel as pixel_type)
        dim as point2d p2d = p3d2d(p3d)
        'circle (p2d.x, p2d.y), 3, pixel.value
        pset(p2d.x, p2d.y), pixel.value
end sub

sub line3d(p3d1 as point3d, p3d2 as point3d, pixel as pixel_type)
        dim as point3d p3dm
        dim as point2d p2d1 = p3d2d(p3d1)
        dim as point2d p2d2 = p3d2d(p3d2)
        p3dm.x = (p3d1.x + p3d2.x) \ 2
        p3dm.y = (p3d1.y + p3d2.y) \ 2
        p3dm.z = (p3d1.z + p3d2.z) \ 2
        scalePixel(pixel, p3dm.x)
        line(p2d1.x, p2d1.y)-(p2d2.x, p2d2.y), pixel.value
end sub

'-------------------- VECTOR -----------------------

function vector_substract(v1 as point3d, v2 as point3d) as point3d
        dim as point3d v
        v.x = v1.x - v2.x
        v.y = v1.y - v2.y
        v.z = v1.z - v2.z
        return v
end function

function vector_add(v1 as point3d, v2 as point3d) as point3d
        dim as point3d v
        v.x = v1.x + v2.x
        v.y = v1.y + v2.y
        v.z = v1.z + v2.z
        return v
end function

function vector_average(v1 as point3d, v2 as point3d) as point3d
        dim as point3d v
        v.x = (v1.x + v2.x) / 2
        v.y = (v1.y + v2.y) / 2
        v.z = (v1.z + v2.z) / 2
        return v
end function

function vector_multiply(v1 as point3d, mul as single) as point3d
        dim as point3d v
        v.x = v1.x * mul
        v.y = v1.y * mul
        v.z = v1.z * mul
        return v
end function

function vector_squared(v1 as point3d) as single
        return v1.x * v1.x + v1.y * v1.y + v1.z * v1.z
end function

function vector_magnitude(v1 as point3d) as single
        return sqr(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z)
end function

declare function vector_normalise (v1 as point3d) as point3d

function vector_normalise(v1 as point3d) as point3d
        dim as single magnitude = sqr(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z)
        dim as point3d v
        v.x = v1.x / magnitude
        v.y = v1.y / magnitude
        v.z = v1.z / magnitude
        return v
end function

function vector_cross_product(v1 as point3d, v2 as point3d) as point3d
        dim as point3d v
        v.x = v1.y * v2.z - v1.z * v2.y
        v.y = v1.z * v2.x - v1.x * v2.z
        v.z = v1.x * v2.y - v1.y * v2.x
        return v
end function

sub vector_print(v1 as point3d)
        print "  x:"; v1.x
        print "  y:"; v1.y
        print "  z:"; v1.z
        print "  magnitude:"; vector_magnitude(v1)
end sub

'-------------------------------------------------

sub rotate(p3d as point3d, xTheta as single, yTheta as single, zTheta as single)
        'From tutorials Relsoft
        dim as single x = p3d.x, y = p3d.y, z = p3d.z
        dim as single xNew, yNew, zNew
        '***Rotation on the Z-axis
        yNew = y*cos(xTheta) - z*sin(xTheta)
        zNew = z*cos(xTheta) + y*sin(xTheta)
        y = yNew
        z = zNew

        '***Rotation on the Y-axis
        zNew = z*cos(yTheta) - x*sin(yTheta)
        xNew = x*cos(yTheta) + z*sin(yTheta)
        x = xNew

        '***Rotation on the Z-axis
        xNew = x*cos(zTheta) - y*sin(zTheta)
        yNew = y*cos(zTheta) + x*sin(zTheta)

        p3d.x = xNew
        p3d.y = yNew
        p3d.z = zNew
end sub

sub translate(p3d as point3d, xTrans as single, yTrans as single, zTrans as single)
        p3d.x += xTrans
        p3d.y += yTrans
        p3d.z += zTrans
end sub

sub multiply(p3d as point3d, xMul as single, yMul as single, zMul as single)
        p3d.x *= xMul
        p3d.y *= yMul
        p3d.z *= zMul
end sub

'-------------------- COIL -----------------------

sub drawCoilSection(p1 as point3d, p2 as point3d)
        dim as point3d p3
        line3d(p1, p2, createPixel(255, 0, 0))
        p3 = vector_average(p1, p2)
        pset3d(p3, createPixel(255, 255, 255))
end sub

type coil_type
        as single current '+/- [A]
        as single radius '[m]
        as single xTrans, yTrans, zTrans '[m]
        as single xTheta, yTheta, zTheta '[rad]
        declare function getPoint (angle as single) as point3d
        declare function getBField(poi as point3d, numSections as integer) as point3d
        declare sub show(numSections as integer)
end type

function coil_type.getPoint (angle as single) as point3d
        dim as point3d p1 = type<point3d> (cos(angle), 0, sin(angle))
        multiply(p1, radius, radius, radius)
        rotate(p1, xTheta, yTheta, zTheta)
        translate(p1, xTrans, yTrans, zTrans)
        return p1
end function

function coil_type.getBField(poi as point3d, numSections as integer) as point3d
        dim as single angle, dAngle
        dim as point3d vector_dl 'short piece of current conductor [m]
        dim as point3d vector_r 'from conductior piece to point of interest
        dim as point3d vector_dB, vector_B '[T] or [V・s・m-2]
        dim as point3d p1, p2
        vector_B = type<point3d> (0, 0, 0)
        dAngle  = pi / numSections
        p2 = getPoint(-pi)
        'loop coil in steps, up to pi() and a bit too prvent gap
        for angle = -pi + dAngle to +pi + dAngle/10 step dAngle
                p1 = p2
                p2 = getPoint(angle)
                'drawCoilSection(p1, p2)
                vector_dl = vector_substract(p2, p1)
                vector_r = vector_substract(poi, p1)
                'vector_dB [T] = u0 [(V*s)/(A*m)] /  4*pi() * _
                '(I [A] * vector_dl [m] cross_product unit_vector_r []) / abs(r^2) [m2]
                vector_dB = _
                vector_multiply(vector_cross_product(vector_multiply(vector_dl, current),        _
                vector_normalise(vector_r)), u0div4pi / vector_squared(vector_r))
                vector_B = vector_add(vector_B, vector_dB)
        next
        return vector_B
end function

sub coil_type.show(numSections as integer)
        dim as single angle, dAngle
        dim as point3d p1, p2
        dAngle  = pi / numSections
        p2 = getPoint(-pi)
        'loop coil in steps, up to pi() and a bit too prvent gap
        for angle = -pi + dAngle to +pi + dAngle/10 step dAngle
                p1 = p2
                p2 = getPoint(angle)
                drawCoilSection(p1, p2)
        next
end sub

'------------------ PARTICLE ---------------------

type particle
        dim as point3d position
        dim as point3d velocity
        dim as point3d accelleration
        dim as point3d force
        dim as single mass
        dim as single charge
end type

'-------------------- MAIN -----------------------

dim shared as point3d p0 = type<point3d> (0, 0, 0)
dim as integer i
dim as point3d vector_B '[T] or [V・s・m-2]
dim as point3d poi 'point of interest
dim as single y, x

dim as graph_type gr1, gr2, gr3

screenres scrn_w, scrn_h, 32

with gr1
  .onScreenOffset.x = 500
  .onScreenOffset.y = 400 'from bottom of screen
  .onScreenSize.x = 450
  .onScreenSize.y = 350
  .dotColour = &hff00ff
  .axisColour = &hc0c0c0
  .gridColour = &h444444
  .physicalMin.x = -1.00 'm
  .physicalMax.x = +1.00 'm
  .physicalMin.y = -2.0 'T
  .physicalMax.y = +2.0 'T
  .init()
  .setGrid()
  .setAxis()
end with

gr2 = gr1

with gr2
  .onScreenOffset.y = 20 'from bottom of screen
  .physicalMin.y = -0.02 'T
  .physicalMax.y = +0.02 'T
  .init()
  .setGrid()
  .setAxis()
end with

gr3  = gr1

with gr3
  .onScreenOffset.x = 20
  .onScreenOffset.y = 20 'from bottom of screen
  .physicalMin.y = -0.1 'm
  .physicalMax.y = +0.1 'm
  .init()
  .setGrid()
  .setAxis()
end with

'Draw axis for coils
for i = -200 to +200
        pset3d(type<point3d> (i / ppm, 0, 0), createPixel(255, 255, 255))
        pset3d(type<point3d> (0, i / ppm, 0), createPixel(255, 255, 255))
        pset3d(type<point3d> (0, 0, i / ppm), createPixel(255, 255, 255))
        draw string (p3d2d(type<point3d>(1, 0, 0)).x, p3d2d(type<point3d>(1, 0, 0)).y), "x", &hd0d0d0
        draw string (p3d2d(type<point3d>(0, 1, 0)).x, p3d2d(type<point3d>(0, 1, 0)).y), "y", &hd0d0d0
        draw string (p3d2d(type<point3d>(0, 0, 1)).x, p3d2d(type<point3d>(0, 0, 1)).y), "z", &hd0d0d0
next

dim as coil_type coil(0 to 3)

for i = lbound(coil) to ubound(coil)
        coil(i).current = 22 * 20 * 1000 '[A]
        coil(i).radius = 0.5 '[m]
next

coil(0).yTrans = -0.40 '[m]
coil(1).yTrans = -0.20 '[m]
coil(2).yTrans = +0.20 '[m]
coil(3).yTrans = +0.40 '[m]

coil(3).xTrans = 0.002 '2 cm
coil(3).zTheta = 0.03 'rad

const as integer numCoilSections = 20

'show the coils
for i = lbound(coil) to ubound(coil)
        coil(i).show(numCoilSections)
next

'show the field
for y = gr1.physicalMin.x to gr1.physicalMax.x step 0.001
        poi = type<point3d> (x, y, 0) 'point of interest, shifted on x
        vector_B = type<point3d> (0, 0, 0)
        for i = lbound(coil) to ubound(coil)
                vector_B = vector_add(vector_B, coil(i).getBField(poi, numCoilSections))
        next

        pset3d(poi, createPixel(255, 255, 0))
        
        gr1.setDotCustom(y, vector_B.x, &h770077)
        gr1.setDotCustom(y, vector_B.y, &h007777)
        gr1.setDotCustom(y, vector_B.z, &h777700)
        
        gr2.setDotCustom(y, vector_B.x, &h770077)
        gr2.setDotCustom(y, vector_B.z, &h777700)

        draw string (gr1.onScreenOffset.x + gr1.onScreenSize.x - 30, scrn_h - (gr1.onScreenOffset.y + gr1.onScreenSize.y - 4)), "B.x", &hff00ff
        draw string (gr1.onScreenOffset.x + gr1.onScreenSize.x - 30, scrn_h - (gr1.onScreenOffset.y + gr1.onScreenSize.y - 14)), "B.y", &h00ffff
        draw string (gr1.onScreenOffset.x + gr1.onScreenSize.x - 30, scrn_h - (gr1.onScreenOffset.y + gr1.onScreenSize.y - 24)), "B.z", &hffff00
next

dim as single t, tMax, tStep
dim as particle ion
dim as particle start
dim as double speed
ion.position.y = -0.50 'm

ion.velocity.y = 520 'm/s 
ion.mass = 1.67e-27     '1.67e-27 'kg (1H+ ion)
ion.charge = 0.6e-25'C = A*s = V*F
start=ion
speed=sqr(ion.velocity.x^2+ion.velocity.y^2+ion.velocity.z^2)
tMax = 1 / ion.velocity.y
tStep = tMax / 10000
print "Initial speed ";speed
for t = 0 to tMax step tStep
        poi = ion.position 'point of interest is now ion location
        marker3d(poi, createPixel(255, 255, 0))
        gr3.setDotCustom(ion.position.y, ion.position.x, &hffff00)
        gr3.setDotCustom(ion.position.y, ion.position.z, &hffffff)

        vector_B = type<point3d> (0, 0, 0)
        for i = lbound(coil) to ubound(coil)
                vector_B = vector_add(vector_B, coil(i).getBField(poi, numCoilSections))
        next

        ion.force = vector_multiply(vector_cross_product(ion.velocity, vector_B), ion.charge) 'q (v x B)
        ion.accelleration = vector_multiply(ion.force, 1.0 / ion.mass) 'a = F / m
        ion.velocity = vector_add(ion.velocity, vector_multiply(ion.accelleration, t)) 'dv = a * dt v=at
        ion.position = vector_add(start.position, vector_multiply(ion.velocity, t)) 'dx = v * dt x=.5*t^2
        ion.position=vector_add(ion.position,vector_multiply(ion.accelleration,+.5*t^2))
        'sleep 1,1
next
speed=sqr(ion.velocity.x^2+ion.velocity.y^2+ion.velocity.z^2)
print "Final   speed ";speed


print "Simulation Ended."
sleep