module.jai

#load "bindings.jai";
// #load "quickhull.jai"; //b3CreateHull already exists...


B3_DEFAULT_CATEGORY_BITS :: U64_MAX;
B3_DEFAULT_MASK_BITS :: U64_MAX;

B3_INTERFACE_HAS_INDEX1 :: struct {
    index1: s32;
};


B3_IS_NULL :: ( id: $T/interface B3_INTERFACE_HAS_INDEX1 ) -> bool {
    return id.index1 == 0;
};

/// Macro to determine if any id is non-null.
B3_IS_NON_NULL :: ( id: $T/interface B3_INTERFACE_HAS_INDEX1 ) -> bool{
    return id.index1 != 0;
};

/// Compare two ids for equality. Doesn't work for b3WorldId. Don't mix types.
// #define B3_ID_EQUALS( id1, id2 ) ( id1.index1 == id2.index1 && id1.world0 == id2.world0 && id1.generation == id2.generation )
B3_ID_EQUALS :: (id1: $T/interface b3BodyId, id2: $U/interface b3BodyId) -> bool {
    return id1.index1 == id2.index1 && id1.world0 == id2.world0 && id1.generation == id2.generation;
};

operator == ::(a: b3BodyId, b: b3BodyId) -> bool {
	return B3_ID_EQUALS(a,b);
}

operator == ::(a: b3JointId, b: b3JointId) -> bool {
	return B3_ID_EQUALS(a,b);
}


b3MulQuat :: ( q1: b3Quat, q2: b3Quat ) -> b3Quat
{
	t1 := cross_product( q1.xyz, q2.xyz );
	t2 := t1+ q1.w * q2.xyz;
	t3 := t2+ q2.w * q1.xyz;
	q := b3Quat.{ xyz=t3, w=q1.w * q2.w - dot_product( q1.xyz, q2.xyz )};
	return q;
}

b3InvMulQuat :: ( q1: b3Quat, q2: b3Quat ) -> b3Quat
{
	t1: b3Vec3 = cross_product( q2.xyz, q1.xyz );
	t2: b3Vec3 =  t1 + q1.w * q2.xyz;
	t3: b3Vec3 =  t2 - q2.w * q1.xyz;
	q: b3Quat = { xyz=t3, w=q1.w * q2.w + dot_product( q1.xyz, q2.xyz ) };
	return q;
}

b3RotateVector :: ( q: b3Quat, v: b3Vec3) -> b3Vec3
{
	// v + 2 * cross(q.xyz, cross(q.xyz, v) + q.w * v)
	// B3_ASSERT( b3IsNormalizedQuat( q ) );
	t1 := cross_product( q.xyz, v );
	t2 :=  t1 + q.w * v;
	t3 := cross_product( q.xyz, t2 );
	return v + 2.0 * t3;
}

/// Transform a local point to a world position. Rotation in float, translation in double.
b3TransformWorldPoint :: (t:  b3WorldTransform, p:b3Vec3 ) -> b3Pos
{
	r: b3Vec3 = b3RotateVector( t.q, p );
	return t.p + r;
}

/// Transform a world position to a local point. One double subtraction, then float.
b3InvTransformWorldPoint :: (t:  b3WorldTransform, p:b3Pos ) -> b3Vec3
{
	d: b3Vec3 = p - t.p;
	return b3InvRotateVector( t.q, d );
}

/// Inverse rotate a vector.
b3InvRotateVector :: ( q: b3Quat, v: b3Vec3) -> b3Vec3
{
	// v + 2 * cross(q.xyz, cross(q.xyz, v) - q.w * v)
	// B3_ASSERT( b3IsNormalizedQuat( q ) );
	t1 := cross_product( q.xyz, v );
	t2 :=  t1 - q.w * v;
	t3 := cross_product( q.xyz, t2 );
	return  v + 2.0 * t3;
}
b3InvertTransform :: ( t: b3Transform) -> b3Transform
{
	out: b3Transform;
	out.p = b3InvRotateVector( t.q, -t.p );
	out.q = t.q;
	out.q.xyz = -out.q.xyz;
	return out;
}


operator* :: ( a: b3Transform, b: b3Transform ) -> b3Transform
{
	out: b3Transform;
	out.p = b3RotateVector( a.q, b.p ) + a.p;
	out.q = b3MulQuat( a.q, b.q );
	return out;
}

b3NormalizeQuat :: ( q: b3Quat ) -> b3Quat
{
	lengthSq: float = b3DotQuat( q, q );
	if ( lengthSq > 1000.0 * FLOAT32_MIN )
	{
		s: float = 1.0 / sqrt( lengthSq );
		qn: b3Quat = {  s * q.xyz.x, s * q.xyz.y, s * q.xyz.z, s * q.w };
		return qn;
	}

	return b3Quat_identity;
}

b3MakeQuatFromAxisAngle :: (  axis: b3Vec3,  radians:float ) -> b3Quat
{
	//B3_ASSERT( b3IsNormalized( axis ) );
	cs :b3CosSin= b3ComputeCosSin( 0.5 * radians );
	q: b3Quat= { { cs.sine * axis.x, cs.sine * axis.y, cs.sine * axis.z }, cs.cosine };
	return q;
}

b3MakeMatrixFromQuat :: inline ( q: b3Quat ) -> b3Matrix3
{
	_xx: float = q.xyz.x * q.xyz.x;
	yy: float = q.xyz.y * q.xyz.y;
	zz: float = q.xyz.z * q.xyz.z;
	xy: float = q.xyz.x * q.xyz.y;
	xz: float = q.xyz.x * q.xyz.z;
	xw: float = q.xyz.x * q.w;
	yz: float = q.xyz.y * q.xyz.z;
	yw: float = q.xyz.y * q.w;
	zw: float = q.xyz.z * q.w;

	return  b3Matrix3.{
		{ 1.0 - 2.0 * ( yy + zz ), 2.0 * ( xy + zw ), 2.0 * ( xz - yw ) },
		{ 2.0 * ( xy - zw ), 1.0 - 2.0 * ( _xx + zz ), 2.0 * ( yz + xw ) },
		{ 2.0 * ( xz + yw ), 2.0 * ( yz - xw ), 1.0 - 2.0 * ( _xx + yy ) },
	};
}


b3MakeQuatFromMatrix :: (m: b3Matrix3 ) -> b3Quat
{
	c1: b3Vec3 = m.v[0];
	c2: b3Vec3 = m.v[1];
	c3: b3Vec3 = m.v[2];

	q: b3Quat;

	trace: float = m.v[0].x + m.v[1].y + m.v[2].z;
	if ( trace >= 0.0 )
	{
		q.xyz.x = c2.z - c3.y;
		q.xyz.y = c3.x - c1.z;
		q.xyz.z = c1.y - c2.x;
		q.w = trace + 1.0;
	}
	else
	{
		if ( c1.x > c2.y && c1.x > c3.z )
		{
			q.xyz.x = c1.x - c2.y - c3.z + 1.0;
			q.xyz.y = c2.x + c1.y;
			q.xyz.z = c3.x + c1.z;
			q.w = c2.z - c3.y;
		}
		else if ( c2.y > c3.z )
		{
			q.xyz.x = c1.y + c2.x;
			q.xyz.y = c2.y - c3.z - c1.x + 1.0;
			q.xyz.z = c3.y + c2.z;
			q.w = c3.x - c1.z;
		}
		else
		{
			q.xyz.x = c1.z + c3.x;
			q.xyz.y = c2.z + c3.y;
			q.xyz.z = c3.z - c1.x - c2.y + 1.0;
			q.w = c1.y - c2.x;
		}
	}

	// The algorithm is simplified and made more accurate by normalizing at the end
	return b3NormalizeQuat( q );
}


b3DotQuat :: ( a: b3Quat, b: b3Quat ) -> float
{
	return a.xyz.x * b.xyz.x + a.xyz.y * b.xyz.y + a.xyz.z * b.xyz.z + a.w * b.w;
}


 #scope_file
 #import "Math";