three.cad/wasm/solver.c

182 lines
5.0 KiB
C
Raw Normal View History

2021-03-21 10:42:14 +00:00
/*-----------------------------------------------------------------------------
* Some sample code for slvs.dll. We draw some geometric entities, provide
* initial guesses for their positions, and then constrain them. The solver
* calculates their new positions, in order to satisfy the constraints.
*
* Copyright 2008-2013 Jonathan Westhues.
*---------------------------------------------------------------------------*/
#ifdef WIN32
2021-03-21 21:39:59 +00:00
#include <windows.h>
2021-03-21 10:42:14 +00:00
#endif
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
2021-03-24 17:49:40 +00:00
#include <math.h>
2021-03-21 10:42:14 +00:00
#include "slvs.h"
static Slvs_System sys;
static void *CheckMalloc(size_t n)
{
2021-03-21 21:39:59 +00:00
void *r = malloc(n);
if (!r)
{
printf("out of memory!\n");
exit(-1);
}
return r;
2021-03-21 10:42:14 +00:00
}
/*-----------------------------------------------------------------------------
* An example of a constraint in 3d. We create a single group, with some
* entities and constraints.
*---------------------------------------------------------------------------*/
2021-03-21 21:39:59 +00:00
2021-04-16 00:05:40 +00:00
int solver(int nPts, float *p_ptr, int nConst, float *c_ptr, int nLinks, float *l_ptr, int geomStartIdx)
2021-03-22 01:57:08 +00:00
{
2021-03-24 17:49:40 +00:00
// printf("first %i \n", (int)*(l_ptr + 1));
2021-03-21 21:39:59 +00:00
Slvs_hGroup g;
double qw, qx, qy, qz;
2021-03-21 10:42:14 +00:00
2021-03-21 21:39:59 +00:00
g = 1;
/* First, we create our workplane. Its origin corresponds to the origin
2021-03-21 10:42:14 +00:00
* of our base frame (x y z) = (0 0 0) */
2021-03-21 21:39:59 +00:00
sys.param[sys.params++] = Slvs_MakeParam(1, g, 0.0);
sys.param[sys.params++] = Slvs_MakeParam(2, g, 0.0);
sys.param[sys.params++] = Slvs_MakeParam(3, g, 0.0);
sys.entity[sys.entities++] = Slvs_MakePoint3d(101, g, 1, 2, 3);
/* and it is parallel to the xy plane, so it has basis vectors (1 0 0)
2021-03-23 03:55:44 +00:00
* and (0 1 0). */
2021-03-21 21:39:59 +00:00
Slvs_MakeQuaternion(1, 0, 0,
0, 1, 0, &qw, &qx, &qy, &qz);
sys.param[sys.params++] = Slvs_MakeParam(4, g, qw);
sys.param[sys.params++] = Slvs_MakeParam(5, g, qx);
sys.param[sys.params++] = Slvs_MakeParam(6, g, qy);
sys.param[sys.params++] = Slvs_MakeParam(7, g, qz);
sys.entity[sys.entities++] = Slvs_MakeNormal3d(102, g, 4, 5, 6, 7);
2021-03-21 10:42:14 +00:00
2021-03-21 21:39:59 +00:00
sys.entity[sys.entities++] = Slvs_MakeWorkplane(200, g, 101, 102);
2021-03-21 10:42:14 +00:00
2021-03-21 21:39:59 +00:00
/* Now create a second group. We'll solve group 2, while leaving group 1
2021-03-21 10:42:14 +00:00
* constant; so the workplane that we've created will be locked down,
* and the solver can't move it. */
2021-03-21 21:39:59 +00:00
g = 2;
/* These points are represented by their coordinates (u v) within the
2021-03-21 10:42:14 +00:00
* workplane, so they need only two parameters each. */
2021-03-24 17:49:40 +00:00
Slvs_hParam ph = 11;
2021-03-22 01:57:08 +00:00
Slvs_hParam vh = 301, vh_s = 301;
Slvs_hParam lh = 400, lh_s = 400;
2021-03-21 10:42:14 +00:00
2021-03-24 17:49:40 +00:00
float *buf_pt_start = p_ptr;
int p_start = sys.params;
for (int i = 0; i < nPts; i++)
2021-03-21 21:39:59 +00:00
{
2021-03-24 17:49:40 +00:00
if (isnan((float)*p_ptr))
{
2021-03-25 08:04:13 +00:00
p_ptr += 3;
2021-03-24 17:49:40 +00:00
continue;
}
2021-04-08 04:05:54 +00:00
2021-03-24 17:49:40 +00:00
sys.param[sys.params++] = Slvs_MakeParam(ph++, g, (float)*p_ptr++);
sys.param[sys.params++] = Slvs_MakeParam(ph++, g, (float)*p_ptr++);
sys.entity[sys.entities++] = Slvs_MakePoint2d(i, g, 200, ph - 1, ph - 2);
2021-03-25 08:04:13 +00:00
p_ptr += 1;
2021-03-24 17:49:40 +00:00
}
2021-03-21 21:39:59 +00:00
2021-03-24 17:49:40 +00:00
for (int i = 0; i < nLinks; i++)
{
2021-03-25 08:04:13 +00:00
switch ((int)*l_ptr++)
2021-03-21 21:39:59 +00:00
{
2021-03-25 08:04:13 +00:00
case 0:
2021-04-03 04:05:28 +00:00
sys.entity[sys.entities++] = Slvs_MakeLineSegment((int)*(l_ptr + 2), g,
2021-03-25 08:04:13 +00:00
200, (int)*l_ptr, (int)*(l_ptr + 1));
break;
case 1:
2021-04-03 04:05:28 +00:00
sys.entity[sys.entities++] = Slvs_MakeArcOfCircle((int)*(l_ptr + 3), g, 200, 102,
2021-03-25 08:04:13 +00:00
(int)*(l_ptr + 2), (int)*(l_ptr), (int)*(l_ptr + 1));
break;
default:
break;
2021-03-21 21:39:59 +00:00
}
2021-03-25 08:04:13 +00:00
l_ptr += 4;
2021-03-24 17:49:40 +00:00
}
2021-04-16 08:12:10 +00:00
int c_pre = 1;
2021-04-08 04:05:54 +00:00
sys.constraint[sys.constraints++] = Slvs_MakeConstraint(
2021-04-16 08:12:10 +00:00
c_pre++, 2,
2021-04-08 04:05:54 +00:00
SLVS_C_POINTS_COINCIDENT,
200,
-1,
2021-04-16 00:05:40 +00:00
101, geomStartIdx, -1, -1);
2021-04-16 08:12:10 +00:00
for (int c_id=c_pre; c_id < c_pre + nConst; c_id++, c_ptr += 6)
2021-03-24 17:49:40 +00:00
{
2021-04-03 04:05:28 +00:00
sys.constraint[sys.constraints++] = Slvs_MakeConstraint(
2021-04-08 04:05:54 +00:00
c_id, g,
2021-04-03 04:05:28 +00:00
(int)*c_ptr + 100000,
200,
*(c_ptr + 1),
(int)*(c_ptr + 2), (int)*(c_ptr + 3), (int)*(c_ptr + 4), (int)*(c_ptr + 5));
2021-03-21 21:39:59 +00:00
}
/* And solve. */
Slvs_Solve(&sys, g);
if (sys.result == SLVS_RESULT_OKAY)
{
2021-03-22 01:57:08 +00:00
// printf("solved okay\n");
2021-03-21 21:39:59 +00:00
2021-03-24 17:49:40 +00:00
for (int i = 0; i < nPts; i++)
2021-03-21 21:39:59 +00:00
{
2021-03-24 17:49:40 +00:00
if (isnan((float)*buf_pt_start))
{
2021-03-25 08:04:13 +00:00
buf_pt_start += 3;
2021-03-24 17:49:40 +00:00
continue;
}
*buf_pt_start++ = (float)sys.param[p_start++].val;
2021-03-22 01:57:08 +00:00
*buf_pt_start++ = (float)sys.param[p_start++].val;
2021-03-25 08:04:13 +00:00
buf_pt_start += 1;
2021-03-21 21:39:59 +00:00
}
}
else
{
int i;
printf("solve failed: problematic constraints are:");
for (i = 0; i < sys.faileds; i++)
{
printf(" %d", sys.failed[i]);
}
printf("\n");
if (sys.result == SLVS_RESULT_INCONSISTENT)
{
printf("system inconsistent\n");
2021-03-21 10:42:14 +00:00
}
2021-03-21 21:39:59 +00:00
else
{
printf("system nonconvergent\n");
}
}
sys.params = sys.constraints = sys.entities = 0;
return 0;
2021-03-21 10:42:14 +00:00
}
int main(int argc, char *argv[])
{
2021-03-24 20:47:12 +00:00
sys.param = CheckMalloc(500 * sizeof(sys.param[0]));
sys.entity = CheckMalloc(500 * sizeof(sys.entity[0]));
sys.constraint = CheckMalloc(500 * sizeof(sys.constraint[0]));
2021-03-21 10:42:14 +00:00
2021-03-24 20:47:12 +00:00
sys.failed = CheckMalloc(500 * sizeof(sys.failed[0]));
sys.faileds = 500;
2021-03-21 10:42:14 +00:00
2021-03-21 21:39:59 +00:00
// Example2d(150.0);
2021-03-21 10:42:14 +00:00
2021-03-21 21:39:59 +00:00
printf("hello\n");
return 0;
2021-03-21 10:42:14 +00:00
}