github.com/egonelbre/exp@v0.0.0-20240430123955-ed1d3aa93911/physicscompress2/physics/state.go (about)

     1  package physics
     2  
     3  import (
     4  	"encoding/gob"
     5  	"io"
     6  	"math"
     7  
     8  	"github.com/egonelbre/exp/bit"
     9  )
    10  
    11  const (
    12  	HistorySize = 16
    13  
    14  	UnitsPerMeter = 512
    15  	UnitsPerQuat  = 1024
    16  
    17  	PositionUnit = 1.0 / UnitsPerMeter
    18  	QuatUnit     = 1.0 / UnitsPerQuat
    19  )
    20  
    21  type State struct {
    22  	History    [HistorySize]*Frame
    23  	FrameIndex int
    24  }
    25  
    26  type Frame struct {
    27  	Cubes []Cube
    28  }
    29  
    30  type Rotation struct{ X, Y, Z, W float32 }
    31  
    32  func same32(delta, precision float32) bool {
    33  	if delta < 0 {
    34  		delta = -delta
    35  	}
    36  	return delta <= precision
    37  }
    38  
    39  func (a Rotation) Equals(b Rotation) bool {
    40  	return same32(a.X-b.X, QuatUnit) &&
    41  		same32(a.Y-b.Y, QuatUnit) &&
    42  		same32(a.Z-b.Z, QuatUnit) &&
    43  		same32(a.W-b.W, QuatUnit)
    44  }
    45  
    46  type Position struct{ X, Y, Z float32 }
    47  
    48  func (a Position) Sub(b Position) Position {
    49  	return Position{
    50  		X: a.X - b.X,
    51  		Y: a.Y - b.Y,
    52  		Z: a.Z - b.Z,
    53  	}
    54  }
    55  
    56  func (p Position) Len() float32 {
    57  	return float32(math.Sqrt(float64(p.X*p.X + p.Y*p.Y + p.Z*p.Z)))
    58  }
    59  
    60  func (a Position) Equals(b Position) bool {
    61  	return same32(a.X-b.X, PositionUnit) &&
    62  		same32(a.Y-b.Y, PositionUnit) &&
    63  		same32(a.Z-b.Z, PositionUnit)
    64  }
    65  
    66  type Cube struct {
    67  	Pos Position
    68  	Rot Rotation
    69  
    70  	Interacting bool
    71  }
    72  
    73  func NewFrame(size int) *Frame { return &Frame{make([]Cube, size)} }
    74  
    75  func (frame *Frame) Assign(other *Frame) {
    76  	frame.Cubes = append([]Cube{}, other.Cubes...)
    77  }
    78  
    79  func (frame *Frame) ReadFrom(r io.Reader) error {
    80  	for i := range frame.Cubes {
    81  		if err := frame.Cubes[i].ReadFrom(r); err != nil {
    82  			return err
    83  		}
    84  	}
    85  	return nil
    86  }
    87  
    88  func (frame *Frame) WriteTo(w io.Writer) error {
    89  	for i := range frame.Cubes {
    90  		if err := frame.Cubes[i].WriteTo(w); err != nil {
    91  			return err
    92  		}
    93  	}
    94  	return nil
    95  }
    96  
    97  func (a *Frame) Equals(b *Frame) bool {
    98  	if len(a.Cubes) != len(b.Cubes) {
    99  		return false
   100  	}
   101  	for i, ax := range a.Cubes {
   102  		bx := b.Cubes[i]
   103  		switch {
   104  		case ax.Interacting != bx.Interacting:
   105  			return false
   106  		case !ax.Pos.Equals(bx.Pos):
   107  			return false
   108  		case !ax.Rot.Equals(bx.Rot):
   109  			return false
   110  		}
   111  	}
   112  	return true
   113  }
   114  
   115  // Cube utilities
   116  func (cube *Cube) ReadFrom(r io.Reader) error {
   117  	interacting := int32(0)
   118  	err := bit.Read(r,
   119  		&cube.Rot.X, &cube.Rot.Y, &cube.Rot.Z, &cube.Rot.W,
   120  		&cube.Pos.X, &cube.Pos.Y, &cube.Pos.Z,
   121  		&interacting,
   122  	)
   123  	cube.Interacting = interacting == 1
   124  	return err
   125  }
   126  
   127  func (cube *Cube) WriteTo(w io.Writer) error {
   128  	interacting := int32(0)
   129  	if cube.Interacting {
   130  		interacting = 1
   131  	}
   132  	return bit.Write(w,
   133  		&cube.Rot.X, &cube.Rot.Y, &cube.Rot.Z, &cube.Rot.W,
   134  		&cube.Pos.X, &cube.Pos.Y, &cube.Pos.Z,
   135  		&interacting,
   136  	)
   137  }
   138  
   139  func NewState(size int) *State {
   140  	s := &State{}
   141  	for i := range s.History {
   142  		s.History[i] = NewFrame(size)
   143  	}
   144  	s.FrameIndex = -1
   145  	return s
   146  }
   147  
   148  func (s *State) IncFrame() {
   149  	s.FrameIndex += 1
   150  }
   151  
   152  func (s *State) ReadNext(r io.Reader) error {
   153  	s.FrameIndex += 1
   154  	return s.Current().ReadFrom(r)
   155  }
   156  
   157  func (s *State) Current() *Frame  { return s.Prev(0) }
   158  func (s *State) Baseline() *Frame { return s.Prev(6) }
   159  func (s *State) Historic() *Frame { return s.Prev(8) }
   160  
   161  func (s *State) Prev(i int) *Frame {
   162  	return s.History[(s.FrameIndex-i+HistorySize)%HistorySize]
   163  }
   164  
   165  func init() {
   166  	gob.Register([]Cube{})
   167  }