From a1c1af2679576d938c8c662805516e12cf217295 Mon Sep 17 00:00:00 2001
From: hgn <hgodden00@gmail.com>
Date: Sun, 15 Jan 2023 19:48:38 +0000
Subject: [PATCH] support maths for constraints

---
 vg_lines.h |  11 ++++++
 vg_m.h     | 102 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 113 insertions(+)

diff --git a/vg_lines.h b/vg_lines.h
index e2a79fe..e71a0e7 100644
--- a/vg_lines.h
+++ b/vg_lines.h
@@ -8,6 +8,17 @@
 
 typedef v3f line_co;
 
+#define VG__RED   0xff0000ff
+#define VG__GREEN 0xff00ff00
+#define VG__BLUE  0xffff0000
+#define VG__WHITE 0xffffffff
+#define VG__BLACK 0xff000000
+#define VG__CLEAR 0x00ffffff
+#define VG__PINK  0xffff00ff
+#define VG__YELOW 0xff00ffff
+#define VG__CYAN  0xffffff00
+#define VG__NONE  0x00000000
+
 static struct vg_shader _shader_lines = 
 {
    .name = "[vg] lines",
diff --git a/vg_m.h b/vg_m.h
index d6da3a1..fbb3c45 100644
--- a/vg_m.h
+++ b/vg_m.h
@@ -146,6 +146,12 @@ static inline void v2_add( v2f a, v2f b, v2f d )
    d[0] = a[0]+b[0]; d[1] = a[1]+b[1];
 }
 
+static inline void v2_abs( v2f a, v2f d )
+{
+   d[0] = fabsf( a[0] );
+   d[1] = fabsf( a[1] );
+}
+
 static inline void v2_muls( v2f a, float s, v2f d )
 {
    d[0] = a[0]*s; d[1] = a[1]*s;
@@ -225,6 +231,19 @@ static inline void v2_floor( v2f a, v2f b )
    b[1] = floorf( a[1] );
 }
 
+static inline void v2_fill( v2f a, float v )
+{
+   a[0] = v;
+   a[1] = v;
+}
+
+/* copysign of b to a */
+static inline void v2_copysign( v2f a, v2f b )
+{
+   a[0] = copysignf( a[0], b[0] );
+   a[1] = copysignf( a[1], b[1] );
+}
+
 /*
  * Vector 3
  */
@@ -243,6 +262,14 @@ static inline void v3_add( v3f a, v3f b, v3f d )
    d[0] = a[0]+b[0]; d[1] = a[1]+b[1]; d[2] = a[2]+b[2];
 }
 
+static inline void v4_add( v4f a, v4f b, v4f d )
+{
+   d[0] = a[0]+b[0]; 
+   d[1] = a[1]+b[1];
+   d[2] = a[2]+b[2];
+   d[3] = a[3]+b[3];
+}
+
 static inline void v3_sub( v3f a, v3f b, v3f d )
 {
    d[0] = a[0]-b[0]; d[1] = a[1]-b[1]; d[2] = a[2]-b[2];
@@ -520,6 +547,27 @@ static inline void m2x2_create_rotation( m2x2f a, float theta )
                         { 0.0f, 0.0f, 0.0f, }}
 
 
+/* a X b == [b]T a == ...*/
+static void m3x3_skew_symetric( m3x3f a, v3f v )
+{
+   a[0][0] =  0.0f;
+   a[0][1] =  v[2];
+   a[0][2] = -v[1];
+   a[1][0] = -v[2];
+   a[1][1] =  0.0f;
+   a[1][2] =  v[0];
+   a[2][0] =  v[1];
+   a[2][1] = -v[0];
+   a[2][2] =  0.0f;
+}
+
+static void m3x3_add( m3x3f a, m3x3f b, m3x3f d )
+{
+   v3_add( a[0], b[0], d[0] );
+   v3_add( a[1], b[1], d[1] );
+   v3_add( a[2], b[2], d[2] );
+}
+
 static inline void m3x3_copy( m3x3f a, m3x3f b )
 {
    v3_copy( a[0], b[0] );
@@ -533,6 +581,14 @@ static inline void m3x3_identity( m3x3f a )
    m3x3_copy( id, a );
 }
 
+static void m3x3_diagonal( m3x3f a, float v )
+{
+   m3x3_identity( a );
+   a[0][0] = v;
+   a[1][1] = v;
+   a[2][2] = v;
+}
+
 static inline void m3x3_zero( m3x3f a )
 {
    m3x3f z = M3X3_ZERO;
@@ -561,6 +617,13 @@ static inline void m3x3_inv( m3x3f src, m3x3f dest )
    dest[2][2] =  (a*e-d*b)*det;
 }
 
+static float m3x3_det( m3x3f m )
+{
+   return   m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])
+          - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0])
+          + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
+}
+
 static inline void m3x3_transpose( m3x3f src, m3x3f dest )
 {
    float a = src[0][0], b = src[0][1], c = src[0][2],
@@ -1648,6 +1711,45 @@ VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
    return k_contact_type_default;
 }
 
+
+static void closest_point_elipse( v2f p, v2f e, v2f o )
+{
+   v2f pabs, ei, e2, ve, t;
+
+   v2_abs( p, pabs );
+   v2_div( (v2f){ 1.0f, 1.0f }, e, ei );
+   v2_mul( e, e, e2 );
+   v2_mul( ei, (v2f){ e2[0]-e2[1], e2[1]-e2[0] }, ve );
+
+   v2_fill( t, 0.70710678118654752f );
+
+   for( int i=0; i<3; i++ )
+   {
+      v2f v, u, ud, w;
+
+      v2_mul( ve, t, v );  /* ve*t*t*t */
+      v2_mul( v, t, v );
+      v2_mul( v, t, v );
+
+      v2_sub( pabs, v, u );
+      v2_normalize( u );
+      
+      v2_mul( t, e, ud );
+      v2_sub( ud, v, ud );
+
+      v2_muls( u, v2_length( ud ), u );
+
+      v2_add( v, u, w );
+      v2_mul( w, ei, w );
+
+      v2_maxv( (v2f){0.0f,0.0f}, w, t );
+      v2_normalize( t );
+   }
+
+   v2_mul( t, e, o );
+   v2_copysign( o, p );
+}
+
 /*
  * Raycasts
  */
-- 
2.25.1