179 lines
6.4 KiB
C
179 lines
6.4 KiB
C
|
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||
|
|
* ##### # # # *
|
||
|
|
* #### ##### ###### # # # # # # # *
|
||
|
|
* # # # # # ## # # # # # *
|
||
|
|
* # # # # ##### # # # # #### # ### *
|
||
|
|
* # # ##### # # # # # # # # # # *
|
||
|
|
* # # # # # ## # # # # # # *
|
||
|
|
* #### # ###### # # ##### ##### # # *
|
||
|
|
* *
|
||
|
|
* This file is part of openGJK. *
|
||
|
|
* *
|
||
|
|
* openGJK is free software: you can redistribute it and/or modify *
|
||
|
|
* it under the terms of the GNU General Public License as published by *
|
||
|
|
* the Free Software Foundation, either version 3 of the License, or *
|
||
|
|
* any later version. *
|
||
|
|
* *
|
||
|
|
* openGJK is distributed in the hope that it will be useful, *
|
||
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
||
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See The *
|
||
|
|
* GNU General Public License for more details. *
|
||
|
|
* *
|
||
|
|
* You should have received a copy of the GNU General Public License *
|
||
|
|
* along with Foobar. If not, see <https://www.gnu.org/licenses/>. *
|
||
|
|
* *
|
||
|
|
* openGJK: open-source Gilbert-Johnson-Keerthi algorithm *
|
||
|
|
* Copyright (C) Mattia Montanari 2018 - 2019 *
|
||
|
|
* http://iel.eng.ox.ac.uk/?page_id=504 *
|
||
|
|
* *
|
||
|
|
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
|
||
|
|
* *
|
||
|
|
* This file runs an example to illustrate how to invoke the openGJK lib *
|
||
|
|
* within a C program. An executable called 'demo' can be compiled with *
|
||
|
|
* CMake. This reads the coordinates of two polytopes from the input *
|
||
|
|
* files userP.dat and userQ.dat, respectively, and returns the minimum *
|
||
|
|
* distance between them computed using the openGJK library. *
|
||
|
|
* *
|
||
|
|
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @file main.c
|
||
|
|
* @author Mattia Montanari
|
||
|
|
* @date April 2018
|
||
|
|
* @brief File illustrating an application that invokes openGJK.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
#define _CRT_HAS_CXX17 0
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
|
||
|
|
/* For importing openGJK this is Step 1: include header in subfolder. */
|
||
|
|
#include "openGJK/openGJK.h"
|
||
|
|
|
||
|
|
#ifndef WIN32
|
||
|
|
#define fscanf_s fscanf
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Function for reading input file with body's coordinates.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
int readinput(const char *inputfile, double ***pts, int * out) {
|
||
|
|
int npoints = 0;
|
||
|
|
int idx = 0;
|
||
|
|
FILE *fp;
|
||
|
|
|
||
|
|
/* Open file. */
|
||
|
|
#ifdef WIN32
|
||
|
|
errno_t err;
|
||
|
|
if ((err = fopen_s(&fp, inputfile, "r")) != 0) {
|
||
|
|
#else
|
||
|
|
if ((fp = fopen(inputfile, "r")) == NULL) {
|
||
|
|
#endif
|
||
|
|
fprintf(stdout, "ERROR: input file %s not found!\n", inputfile);
|
||
|
|
fprintf(stdout, " -> The file must be in the folder from which this program is launched\n\n");
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Read number of input vertices. */
|
||
|
|
if (fscanf_s(fp, "%d", &npoints) != 1)
|
||
|
|
return 1;
|
||
|
|
|
||
|
|
/* Allocate memory. */
|
||
|
|
double **arr = (double **)malloc(npoints * sizeof(double *));
|
||
|
|
for (int i = 0; i < npoints; i++)
|
||
|
|
arr[i] = (double *)malloc(3 * sizeof(double));
|
||
|
|
|
||
|
|
/* Read and store vertices' coordinates. */
|
||
|
|
for (idx = 0; idx < npoints; idx++)
|
||
|
|
{
|
||
|
|
if (fscanf_s(fp, "%lf %lf %lf\n", &arr[idx][0], &arr[idx][1], &arr[idx][2]) != 3)
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Close file. */
|
||
|
|
fclose(fp);
|
||
|
|
|
||
|
|
/* Pass pointers. */
|
||
|
|
*pts = arr;
|
||
|
|
*out = idx;
|
||
|
|
|
||
|
|
return (0);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/**
|
||
|
|
* @brief Main program of example1_c (described in Section 3.1 of the paper).
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
int main() {
|
||
|
|
/* Squared distance computed by openGJK. */
|
||
|
|
double dd;
|
||
|
|
/* Structure of simplex used by openGJK. */
|
||
|
|
struct simplex s;
|
||
|
|
/* Number of vertices defining body 1 and body 2, respectively. */
|
||
|
|
int nvrtx1,
|
||
|
|
nvrtx2;
|
||
|
|
/* Structures of body 1 and body 2, respectively. */
|
||
|
|
struct bd bd1;
|
||
|
|
struct bd bd2;
|
||
|
|
/* Specify name of input files for body 1 and body 2, respectively. */
|
||
|
|
char inputfileA[40] = "userP.dat",
|
||
|
|
inputfileB[40] = "userQ.dat";
|
||
|
|
/* Pointers to vertices' coordinates of body 1 and body 2, respectively. */
|
||
|
|
double(**vrtx1) = NULL,
|
||
|
|
(**vrtx2) = NULL;
|
||
|
|
|
||
|
|
/* For importing openGJK this is Step 2: adapt the data structure for the
|
||
|
|
* two bodies that will be passed to the GJK procedure. */
|
||
|
|
|
||
|
|
/* Import coordinates of object 1. */
|
||
|
|
if (readinput(inputfileA, &vrtx1, &nvrtx1))
|
||
|
|
return (1);
|
||
|
|
bd1.coord = vrtx1;
|
||
|
|
bd1.numpoints = nvrtx1;
|
||
|
|
|
||
|
|
/* Import coordinates of object 2. */
|
||
|
|
if (readinput(inputfileB, &vrtx2, &nvrtx2))
|
||
|
|
return (1);
|
||
|
|
bd2.coord = vrtx2;
|
||
|
|
bd2.numpoints = nvrtx2;
|
||
|
|
|
||
|
|
/* Initialise simplex as empty */
|
||
|
|
s.nvrtx = 0;
|
||
|
|
|
||
|
|
#ifdef DEBUG
|
||
|
|
/* Verify input of body A. */
|
||
|
|
for (int i = 0; i < bd1.numpoints; ++i) {
|
||
|
|
printf("%.2f ", vrtx1[i][0]);
|
||
|
|
printf("%.2f ", vrtx1[i][1]);
|
||
|
|
printf("%.2f\n", bd1.coord[i][2]);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Verify input of body B. */
|
||
|
|
for (int i = 0; i < bd2.numpoints; ++i) {
|
||
|
|
printf("%.2f ", bd2.coord[i][0]);
|
||
|
|
printf("%.2f ", bd2.coord[i][1]);
|
||
|
|
printf("%.2f\n", bd2.coord[i][2]);
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/* For importing openGJK this is Step 3: invoke the GJK procedure. */
|
||
|
|
/* Compute squared distance using GJK algorithm. */
|
||
|
|
dd = gjk(bd1, bd2, &s);
|
||
|
|
|
||
|
|
/* Print distance between objects. */
|
||
|
|
printf("Distance between bodies %f\n", dd);
|
||
|
|
|
||
|
|
/* Free memory */
|
||
|
|
for (int i = 0; i < bd1.numpoints; i++)
|
||
|
|
free(bd1.coord[i]);
|
||
|
|
free(bd1.coord);
|
||
|
|
for (int i = 0; i < bd2.numpoints; i++)
|
||
|
|
free(bd2.coord[i]);
|
||
|
|
free(bd2.coord);
|
||
|
|
|
||
|
|
return (0);
|
||
|
|
}
|