dump.c

来自「细胞自动机的一个源代码」· C语言 代码 · 共 118 行

/*dampening functions
 */

#include "dump.h"

/**************************/

static double dumplend(double *l) {
  return( sqrt((l[0]*l[0])+(l[1]*l[1])+(l[2]*l[2])) );
}

static void dumpnormd(double *l) {
  double len;
  len = dumplend(l);
  l[0]/=len;
  l[1]/=len;
  l[2]/=len;
}

static double dumpdotd(double *a, double *b) {
  return( (a[0]*b[0]) + (a[1]*b[1]) + (a[2]*b[2]) );
}

/**************************/

void dumpsimpinitd(dumpd *d, double c[3], double trgt[3], double delt) {
  int i;
  for (i=0; i<3; i++) {
    d->cur[i]=c[i];
    d->trgt[i]=trgt[i];
  }
  d->delt=delt;
}

void dumpsprginitd(dumpd *d, double c[3], double trgt[3],
	       double kp, double ks, double slen) {
  int i, j;

  for (i=0; i<3; i++) {
    d->cur[i]=c[i];
    d->trgt[i]=trgt[i];
    d->prevtrgt[i]=trgt[i];
  }
  d->kp=kp;
  d->ks=ks;
  d->slen=slen;
}

void dumpsimpinit3d(dumpd *d, double cx, double cy, double cz,
		    double tx, double ty, double tz, double delt) {
  double a[3], b[3];
  a[0]=cx; a[1]=cy; a[2]=cz; b[0]=tx; b[1]=ty; b[2]=tz;
  dumpsimpinitd(d, a, b, delt);
}

void dumpsprnginit3d(dumpd *d, double cx, double cy, double cz,
		     double tx, double ty, double tz,
		     double ks, double kp, double slen) {
  double a[3], b[3];
  a[0]=cx; a[1]=cy; a[2]=cz; b[0]=tx; b[1]=ty; b[2]=tz;
  dumpsprginitd(d, a, b, ks, kp, slen);
}

/* simple dampening */
void dumpsimpd(dumpd *d) {
  d->cur[0] += ((d->trgt[0]) - (d->cur[0]))/(d->delt);
  d->cur[1] += ((d->trgt[1]) - (d->cur[1]))/(d->delt);
  d->cur[2] += ((d->trgt[2]) - (d->cur[2]))/(d->delt);
}

/* spring dampening */
void dumpsprgd(dumpd *d, double delt) {
  double disp[3], vel[3];
  double displen;
  double fmag;

  disp[0] = ((d->cur[0])-(d->trgt[0]));
  disp[1] = ((d->cur[1])-(d->trgt[1]));
  disp[2] = ((d->cur[2])-(d->trgt[2]));

  vel[0] = ((d->prevtrgt[0])-(d->trgt[0]))/(d->delt);
  vel[1] = ((d->prevtrgt[1])-(d->trgt[1]))/(d->delt);
  vel[2] = ((d->prevtrgt[2])-(d->trgt[2]))/(d->delt);

  displen = dumplend(disp);

  fmag = ( ((d->ks)*((d->slen)-(displen))) +
	   ((d->kp)*dumpdotd(disp, vel)/displen) );

  dumpnormd(disp);

  d->cur[0] += disp[0]*fmag*delt;
  d->cur[1] += disp[1]*fmag*delt;
  d->cur[2] += disp[2]*fmag*delt;
}

/*
int main(void) {
  dumpd s, g;
  double cur[] = {0.0, 0.0, 0.0};
  double dest[] = {1.0, 1.0, 1.0};

  dumpsimpinitd(&s, cur, dest, 10.0);
  dumpsprginitd(&g, cur, dest, 4.5, 3.5, .0001);

  while (s.cur[0]<0.99) {
    printf("%f, %f, %f\n", s.cur[0], s.cur[1], s.cur[2]);
    dumpsimpd(&s);
  }

  while (g.cur[0]<0.99) {
    printf("%f, %f, %f\n", g.cur[0], g.cur[1], g.cur[2]);
    dumpsprgd(&g, .1);
  }

}
*/