inline void pointdynamics(point_t *curpt) // Calculate point movement
{
GLdouble r = vnorm(&curpt->pos);
vector_t grav;
vector_t rotf;
vector_t coriolis;
if(r > c_Re) { // Points outside Earth
grav = vscale(-c_Ge * curpt->m / pow(r,3), &curpt->pos);
vaddto(&curpt->f, &grav);
} else { // Take care of points that
vector_t scaled; // are inside Earth
curpt->pos = vscale(c_Re / vnorm(&curpt->pos), &curpt->pos);
scaled = vscale(-vdot(&curpt->pos, &curpt->v) / c_Re / c_Re, &curpt->pos);
vaddto(&curpt->v, &scaled);
curpt->v = vscale(0, &curpt->v);
}
rotf = vscale(c_Vang * c_Vang * curpt->m, &curpt->pos);
rotf.z = 0;
vset(&coriolis, 2 * curpt->v.y * curpt->m * c_Vang, -2 * curpt->v.x * curpt->m * c_Vang, 0);
vaddto(&curpt->f, &rotf);
vaddto(&curpt->f, &coriolis);
vaddscaled(&curpt->v, TIMESTEP / curpt->m, &curpt->f);
vaddscaled(&curpt->pos, TIMESTEP, &curpt->v);
if(curtime <= 0) { curpt->v = vscale(.99, &curpt->v); }
}
void UpdateCamera() {
vsub(&cameraPtr->dir, &cameraPtr->target, &cameraPtr->orig);
vnorm(&cameraPtr->dir);
const Vec up = {0.f, 1.f, 0.f};
const float fov = (M_PI / 180.f) * 45.f;
vxcross(&cameraPtr->x, &cameraPtr->dir, &up);
vnorm(&cameraPtr->x);
vsmul(&cameraPtr->x, width * fov / height, &cameraPtr->x);
vxcross(&cameraPtr->y, &cameraPtr->x, &cameraPtr->dir);
vnorm(&cameraPtr->y);
vsmul(&cameraPtr->y, fov, &cameraPtr->y);
}
msym_error_t findSymmetryOperations(int esl, msym_equivalence_set_t es[], msym_thresholds_t *t, int *lsops, msym_symmetry_operation_t **sops){
msym_error_t ret = MSYM_SUCCESS;
msym_symmetry_operation_t *rsops = NULL;
int lrsops = 0;
for(int i = 0; i < esl;i++){
int llsops = lrsops;
if(MSYM_SUCCESS != (ret = findEquivalenceSetSymmetryOperations(&es[i], t, &lrsops, &rsops))) goto err;
if(llsops > 0 && lrsops == 0) {
free(rsops);
rsops = NULL;
break;
}
}
for(int i = 0;i < lrsops;i++){
vnorm(rsops[i].v);
}
*lsops = lrsops;
*sops = rsops;
return ret;
err:
free(rsops);
*sops = NULL;
*lsops = 0;
return ret;
}
void calcnormals(Gobject *o, int mallocnormals){
int aktpoly = -1,
edges,i1,i2,i3,
normanz = 0,
aktnorm = 0;
vec norm;
double erg;
while ((edges = o->polygons[++aktpoly])!=0){
normanz++;
aktpoly+=edges+2;
}
/* printf("Calcnormals.Normanzahl:%i\n",normanz);*/
if (mallocnormals)
o->na=malloc(sizeof(vec)*normanz);
aktpoly=0;
for (aktnorm=0;aktnorm<normanz;aktnorm++){
i1=o->polygons[aktpoly+3];
i2=o->polygons[aktpoly+4];
i3=o->polygons[aktpoly+5];
veq(norm,vnorm(vp(vdiff(o->va[i2],o->va[i1]),vdiff(o->va[i3],o->va[i1]))));
if (o->type==V3DPOLYHEDRON){
erg=sp(vdiff(o->va[i1],o->posmc),norm);vreset();
if (erg<0) veq(norm,sm(-1,norm));
if (erg==0){printf("Fehler in calcnormals: Normale nicht definiert\n"); }
}
/* pv(norm);*/
veq(o->na[aktnorm],norm);
o->polygons[aktpoly+1]=aktnorm;
aktpoly+=o->polygons[aktpoly]+3;
}
}
void SlideNavmesh::update(const float dt)
{
if (!m_update && !m_step)
return;
m_step = false;
const float maxSpeed = 1.0f;
NavmeshAgent* agent = &m_scene.agents[0];
// Find next corner to steer to.
// Smooth corner finding does a little bit of magic to calculate spline
// like curve (or first tangent) based on current position and movement direction
// next couple of corners.
float corner[2],dir[2];
int last = 1;
vsub(dir, agent->pos, agent->oldpos); // This delta handles wall-hugging better than using current velocity.
vnorm(dir);
vcpy(corner, agent->pos);
if (m_moveMode == AGENTMOVE_SMOOTH || m_moveMode == AGENTMOVE_DRUNK)
last = agentFindNextCornerSmooth(agent, dir, m_scene.nav, corner);
else
last = agentFindNextCorner(agent, m_scene.nav, corner);
if (last && vdist(agent->pos, corner) < 0.02f)
{
// Reached goal
vcpy(agent->oldpos, agent->pos);
vset(agent->dvel, 0,0);
vcpy(agent->vel, agent->dvel);
return;
}
vsub(agent->dvel, corner, agent->pos);
// Apply style
if (m_moveMode == AGENTMOVE_DRUNK)
{
agent->t += dt*4;
float amp = cosf(agent->t)*0.25f;
float nx = -agent->dvel[1];
float ny = agent->dvel[0];
agent->dvel[0] += nx * amp;
agent->dvel[1] += ny * amp;
}
// Limit desired velocity to max speed.
const float distToTarget = vdist(agent->pos,agent->target);
const float clampedSpeed = maxSpeed * min(1.0f, distToTarget/agent->rad);
vsetlen(agent->dvel, clampedSpeed);
vcpy(agent->vel, agent->dvel);
// Move agent
vscale(agent->delta, agent->vel, dt);
float npos[2];
vadd(npos, agent->pos, agent->delta);
agentMoveAndAdjustCorridor(&m_scene.agents[0], npos, m_scene.nav);
}
void UpdateRenderingCPU(void) {
double startTime = WallClockTime();
const float invWidth = 1.f / width;
const float invHeight = 1.f / height;
int x, y;
for (y = 0; y < height; y++) { /* Loop over image rows */
for (x = 0; x < width; x++) { /* Loop cols */
const int i = (height - y - 1) * width + x;
const int i2 = 2 * i;
const float r1 = GetRandom(&seeds[i2], &seeds[i2 + 1]) - .5f;
const float r2 = GetRandom(&seeds[i2], &seeds[i2 + 1]) - .5f;
const float kcx = (x + r1) * invWidth - .5f;
const float kcy = (y + r2) * invHeight - .5f;
Vec rdir;
vinit(rdir,
camera.x.x * kcx + camera.y.x * kcy + camera.dir.x,
camera.x.y * kcx + camera.y.y * kcy + camera.dir.y,
camera.x.z * kcx + camera.y.z * kcy + camera.dir.z);
Vec rorig;
vsmul(rorig, 0.1f, rdir);
vadd(rorig, rorig, camera.orig)
vnorm(rdir);
const Ray ray = {rorig, rdir};
Vec r;
RadiancePathTracing(spheres, sphereCount, &ray,
&seeds[i2], &seeds[i2 + 1], &r);
if (currentSample == 0)
colors[i] = r;
else {
const float k1 = currentSample;
const float k2 = 1.f / (k1 + 1.f);
colors[i].x = (colors[i].x * k1 + r.x) * k2;
colors[i].y = (colors[i].y * k1 + r.y) * k2;
colors[i].z = (colors[i].z * k1 + r.z) * k2;
}
pixels[y * width + x] = toInt(colors[i].x) |
(toInt(colors[i].y) << 8) |
(toInt(colors[i].z) << 16);
}
}
const float elapsedTime = WallClockTime() - startTime;
const float sampleSec = height * width / elapsedTime;
sprintf(captionBuffer, "Rendering time %.3f sec (pass %d) Sample/sec %.1fK\n",
elapsedTime, currentSample, sampleSec / 1000.f);
currentSample++;
}
/** \brief thresholding as proposed in the Matlab Wavelet-Toolbox.
\ingroup grpwavelet
\code
case 'heursure'
hthr = sqrt(2*log(n));
eta = (norm(x).^2-n)/n;
crit = (log(n)/log(2))^(1.5)/sqrt(n);
if eta < crit
thr = hthr;
else
thr = min(thselect(x,'rigrsure'),hthr);
end
\endcode
*/
double heuristic_sure(const double *data, int n){
dprintf("Db: heuristic_sure\n");
double hthr, etat, crit;
hthr = sqrt(2*log(n));
etat = (pow(vnorm(data, n, 2), 2)-n)/(double)n;
crit = pow((log(n)/log(2)), 1.5)/sqrt(n);
if(etat < crit)
return hthr;
else
return fmin(sureshrink(data, n), hthr);
}
/* bond energy 1/2 k (r - r0)^2 */
static double potbond(double *a, double *b,
double r0, double kr, double *fa, double *fb)
{
double dx[D], r, dr, amp;
r = vnorm( vdiff(dx, a, b) );
dr = r - r0;
amp = kr * dr / r;
vsinc(fa, dx, -amp);
vsinc(fb, dx, amp);
return 0.5 * kr * dr * dr;
}
int le_save_task(le_task *t, const char *file)
{
int i, j;
FILE *fp = fopen(file, "w");
if (fp == NULL) {
perror("Failed to open file");
return 1;
}
fprintf(fp, "# vtk DataFile Version 3.0\n");
fprintf(fp, "Created by le_save_task\n");
fprintf(fp, "BINARY\n");
fprintf(fp, "DATASET STRUCTURED_POINTS\n");
fprintf(fp, "DIMENSIONS %d %d 1\n", t->n.x, t->n.y);
fprintf(fp, "SPACING %f %f 0.0\n", t->h.x, t->h.y);
fprintf(fp, "ORIGIN 0.0 0.0 0.0\n");
fprintf(fp, "POINT_DATA %d\n", t->n.x * t->n.y);
/* velocity */
fprintf(fp, "SCALARS v float 1\n");
fprintf(fp, "LOOKUP_TABLE v_table\n");
for (j = 0; j < t->n.y; j++) {
for (i = 0; i < t->n.x; i++) {
float v;
if (t->stype == ST_AOS) {
v = vnorm(gind(i, j).v);
} else if (t->stype == ST_SOA) {
le_vec2 vt = { soa_vx(i, j), soa_vy(i, j) };
v = vnorm(vt);
} else {
assert(0);
}
write_float(fp, v);
}
}
/*
* You can use the same code for saving other variables.
*/
fclose(fp);
return 0;
}
void simul()
{
for(int i = 0; i < numpoints; i++) { vset(&points[i].f, 0, 0, 0); }
for(int i = 0; i < numpoints - 1; i++) {
vector_t vdiff;
GLdouble axialv, frict, elast;
vector_t diff = vsub(&points[i+1].pos, &points[i].pos);
GLdouble l = vnorm(&diff);
vector_t f;
elast = points[i].k * (1 - points[i].initlen / l);
if(elast < 0) { elast = 0; }
points[i].tension = elast * l;
vdiff = vsub(&points[i+1].v, &points[i].v);
axialv = vdot(&vdiff, &diff) / l;
frict = axialv / l * points[i].frict;
f = vscale(elast + frict, &diff);
vaddto(&points[i].f, &f);
vsubfrom(&points[i+1].f, &f);
}
for(int i = 0; i < numpoints; i++) {
double loading = points[i].tension / points[i].crosssect / c_Slim;
if(loading > 1 && curtime > 0) {
points[i].k = 0;
points[i].frict = 0;
}
}
for(int i = 0; i < numpoints; i++) { pointdynamics(&points[i]); }
{
points[0].pos.x = c_Re * cos(0 / 180.0 * M_PI);
points[0].pos.y = 0;
points[0].pos.z = c_Re * sin(0 / 180.0 * M_PI);
vset(&points[0].v, 0, 0, 0);
if(curtime > 1) {
points[BREAKPOINT].k = 0;
points[BREAKPOINT].frict = 0;
}
}
curtime += TIMESTEP;
}
double DistanceTransform::distance2Triangle(const Point3f& pnt, int nt, Point3f& nearPnt)
{
double dist, temp[3];
Point3f v0, v1, v2;
p_Surf->getTriVerts(nt, v0, v1, v2);
Vector3f norm;
p_Surf->getTriNormal(nt, norm);
vector3 vnorm(norm[0], norm[1], norm[2]), vdiff;
Point3f edgePnt;
//1) First compute the shortest distance from the pnt to the plane of the Triangle.
vdiff[0] = pnt[0] - v0[0];
vdiff[1] = pnt[1] - v0[1];
vdiff[2] = pnt[2] - v0[2];
dist = DotProduct(vdiff, vnorm);
nearPnt[0] = pnt[0] - dist * norm[0];
nearPnt[1] = pnt[1] - dist * norm[1];
nearPnt[2] = pnt[2] - dist * norm[2];
//2) Then check if the projected point is within the triangle or not.
// if yes, then the above is the correct shortest distance.
if(pointInTriangle(nearPnt, v0, v1, v2, norm)) {
return fabs(dist);
}
//3) now, the closest point must on the edge.
// find the nearest point on each edge.
dist = distance2Edge(pnt, v0, v1, nearPnt);
double edgeDist = distance2Edge(pnt, v1, v2, edgePnt);
if(edgeDist < dist) {
dist = edgeDist;
nearPnt = edgePnt;
}
edgeDist = distance2Edge(pnt, v2, v0, edgePnt);
if(edgeDist < dist) {
dist = edgeDist;
nearPnt = edgePnt;
}
assert(dist >= 0);
return dist;
}
void draw_thick_curve_segment(float t, vector *pos, vector *dir, void *args) {
draw_thick_curve_segment_args *dtcsargs=(draw_thick_curve_segment_args*) args;
// Compute a vector from the point pos to the edge of the current segment:
vector toedge;
vcopy_as(&toedge, dir);
vrotz(&toedge, M_PI_2);
vnorm(&toedge);
float width = (*(dtcsargs->width_func))(t, dtcsargs->width_param);
vscale(&toedge, width/2.0);
curve segment; // actually just a straight line...
segment.from.x = pos->x - toedge.x; // from one edge
segment.from.y = pos->y - toedge.y;
segment.from.z = 0;
segment.come_from.x = pos->x - toedge.x;
segment.come_from.y = pos->y - toedge.y;
segment.come_from.z = 0;
segment.go_towards.x = pos->x + toedge.x; // to the other
segment.go_towards.y = pos->y + toedge.y;
segment.go_towards.z = 0;
segment.to.x = pos->x + toedge.x;
segment.to.y = pos->y + toedge.y;
segment.to.z = 0;
// Draw our segment:
draw_curve(
dtcsargs->tx,
&segment,
dtcsargs->color
);
// Draw the shade pixel:
tx_set_px(
dtcsargs->tx,
dtcsargs->shade_color,
(size_t) segment.from.x,
(size_t) segment.from.y
);
}
void contour3d(Contour* c){
int i,j,*k;
vecp v;
matp m;
if (c->initialized==0){
c->tva=malloc((c->vaakt+1)*sizeof(vec));
c->na=malloc((c->polakt+1)*sizeof(vec));
c->sp=malloc((c->polakt+1)*sizeof(vec));
c->points=malloc((c->polakt+1)*sizeof(DPoint));
c->Garray=malloc((c->polakt+1)*sizeof(Gobject));
for (i=0;i<=c->polakt;i++) {
c->Garray[i].na=c->na;
c->Garray[i].tva=c->tva;
c->Garray[i].poswc=(vecp) &c->sp[i]; /* compiler distinguishes between
vec* and vecp */
c->Garray[i].points=c->points;
c->Garray[i].polygons=&c->pol[i*6];
c->Garray[i].drawstruct=drawtrianglelight;
j=c->pol[i*6];
veq(c->na[i],vnorm(vp(vdiff(c->va[j+4],c->va[j+3]),vdiff(c->va[j+5],c->va[j+3]))));
}
c->initialized=1;
}
/* Transformation, die jedes Mal durchlaufen wird. */
meq(c->mc2wcmn,getmc2wcmn());
meq(c->mc2wcm,getmc2wcm());
veq(c->mc2wcv,getmc2wcv());
m=c->mc2wcm;
v=c->mc2wcv;
for (i=0;i<=c->vaakt;i++) {
veq(c->tva[i],vsum(matvecmul(m,c->va[i]),v));
wc2dc(&c->points[i].x,&c->points[i].y,c->tva[i]);
}
for (i=0;i<=c->polakt;i++){ /* Schwerpunkt in Weltkoordinaten */
k=&c->pol[i*6+3];
veq(c->sp[i],sm(0.33333,
vsum(vsum(c->tva[*k],c->tva[*(k+1)]),c->tva[*(k+2)])));
insertGobject(&c->Garray[i]);
}
}
void geRK4ApplyGravity(ge_RK4State* s1, ge_RK4State* s2){
const double G = 6.67234E-11;
double d = vdist(s1->position, s2->position);
if(d <= 1.0 || d < s1->radius_min || d < s2->radius_min){
return;
}
double P = G * ((s1->mass * s2->mass) / (d * d));
ge_Vector3d force, f1, f2;
force.x = s2->position.x - s1->position.x;
force.y = s2->position.y - s1->position.y;
force.z = s2->position.z - s1->position.z;
force = vnorm(force);
force = vmulk(force, P);
f1.x = force.x;
f1.y = force.y;
f1.z = force.z;
f2.x = -force.x;
f2.y = -force.y;
f2.z = -force.z;
geRK4ApplyForce(s1, f1);
geRK4ApplyForce(s2, f2);
}
void le_set_ball(le_task *t, const le_vec2 c, const real r, const real s)
{
int i, j;
for (i = 0; i < t->n.x; i++) {
for (j = 0; j < t->n.y; j++) {
le_vec2 x = {t->h.x * i, t->h.y * j};
le_vec2 d = {x.x - c.x, x.y - c.y};
if (vnorm(d) < r) {
/*
* Set pressure disturbance,
*/
if (t->stype == ST_AOS) {
gind(i, j).s.xx = s;
gind(i, j).s.yy = s;
} else if (t->stype == ST_SOA) {
soa_sxx(i, j) = s;
soa_syy(i, j) = s;
} else {
assert(0);
}
}
}
}
}
int
pt_in_gt (struct pt *p, struct groundtri *gt)
{
double theta;
struct vect v1, v2;
theta = 0;
psub (&v1, >->p1, p);
vnorm (&v1, &v1);
psub (&v2, >->p2, p);
vnorm (&v2, &v2);
theta += acos (vdot (&v1, &v2));
psub (&v1, >->p2, p);
vnorm (&v1, &v1);
psub (&v2, >->p3, p);
vnorm (&v2, &v2);
theta += acos (vdot (&v1, &v2));
psub (&v1, >->p3, p);
vnorm (&v1, &v1);
psub (&v2, >->p1, p);
vnorm (&v2, &v2);
theta += acos (vdot (&v1, &v2));
if (theta >= DTOR ((360 - 1e-7))) {
return (1);
}
return (0);
}
void
grounded_moving (void)
{
double now, dt;
struct pt newpos;
struct groundtri *gt;
struct vect ctrl_vel;
now = get_secs ();
dt = now - player.lasttime;
ctrl_vel.x = 0;
ctrl_vel.y = 0;
ctrl_vel.z = 0;
if (fabs (player.p.z - ground_height (&player.p)) <= 1e6) {
switch (player.moving) {
case BACK:
ctrl_vel.x = -.3 * player.speed * cos (player.theta);
ctrl_vel.y = -.3 * player.speed * sin (player.theta);
break;
case FORW:
ctrl_vel.x = player.speed * cos (player.theta);
ctrl_vel.y = player.speed * sin (player.theta);
break;
case SIDE_Q:
ctrl_vel.x = player.speed
* cos (player.theta + DTOR (90));
ctrl_vel.y = player.speed
* sin (player.theta + DTOR (90));
break;
case SIDE_E:
ctrl_vel.x = player.speed
* cos (player.theta - DTOR (90));
ctrl_vel.y = player.speed
* sin (player.theta - DTOR (90));
break;
case FORW_Q:
ctrl_vel.x = player.speed
* cos (player.theta + DTOR (45));
ctrl_vel.y = player.speed
* sin (player.theta + DTOR (45));
break;
case FORW_E:
ctrl_vel.x = player.speed
* cos (player.theta - DTOR (45));
ctrl_vel.y = player.speed
* sin (player.theta - DTOR (45));
break;
case BACK_Q:
ctrl_vel.x = -.3 * player.speed
* cos (player.theta - DTOR (45));
ctrl_vel.y = -.3 * player.speed
* sin (player.theta - DTOR (45));
break;
case BACK_E:
ctrl_vel.x = -.3 * player.speed
* cos (player.theta + DTOR (45));
ctrl_vel.y = -.3 * player.speed
* sin (player.theta + DTOR (45));
break;
}
}
newpos.x = player.p.x + ctrl_vel.x * dt;
newpos.y = player.p.y + ctrl_vel.y * dt;
newpos.z = player.p.z + ctrl_vel.z * dt;
psub (&player.vel, &newpos, &player.p);
vscal (&player.vel, &player.vel, 1 / dt);
if ((gt = detect_plane (&newpos)) == NULL) {
printf ("Can't find ground, moving player to start position "
"to avoid seg fault\n");
player.p.x = 0;
player.p.y = 0;
player.p.z = ground_height (&player.p);
player.movtype = GROUNDED;
player.loc = detect_plane (&player.p);
return;
}
if (gt->theta < DTOR (50)) {
newpos.z = ground_height (&newpos);
psub (&player.vel, &newpos, &player.p);
vscal (&player.vel, &player.vel, 1 / dt);
player.p = newpos;
} else if (newpos.z > gt->pl.middle.z) {
//imperfect detection of cliffs but tolerable for now
player.p = newpos;
player.movtype = FALLING;
printf ("switching to falling\n");
} else {
if (gt == player.loc) {
printf ("line %d: HORRIBLE MESSINESS\n", __LINE__);
paused = YES;
printf ("paused game\n");
return;
}
struct vect v1, v2;
vnorm (&v2, &ctrl_vel);
vcross (&v1, &player.loc->normv, >->normv);
vnorm (&v1, &v1);
if (vdot (&v1, &v2) < 0)
vscal (&v1, &v1, -1);
vscal (&ctrl_vel, &v1, vdot (&ctrl_vel, &v1));
newpos.x = player.p.x + ctrl_vel.x * dt;
newpos.y = player.p.y + ctrl_vel.y * dt;
newpos.z = player.p.z + ctrl_vel.z * dt;
newpos.z = ground_height (&newpos);
psub (&player.vel, &newpos, &player.p);
vscal (&player.vel, &player.vel, 1 / dt);
player.p = newpos;
}
}
/******************************************************************
* Solve Inverse Kinematics
******************************************************************/
double sKinematics::solveIKPalmItr(double x_dest[], int axis, double dir_dest[], double q_rest[], double q_out[])
{
int i,j;
double *q_old, *q_curr, *q_delta, *q_null;
double x[3], dir[3];
double ox[6];
double error_old, error;
error_old = 1000.0; // set maximum error;
q_old = svector(dof);
q_curr = svector(dof);
q_delta= svector(dof);
q_null = svector(dof);
while(1){
getJacobianPalm(axis, OJ );
getEndPos(x);
getEndDirAxis(axis, dir);
getJoints(q_curr);
for( i=0; i<3; i++ ){
ox[i ] = x_dest[i]-x[i];
ox[i+3] = dir_dest[i] - dir[i];
}
error = vnorm(6, ox );
//printf("%f \n", error );
// stop when the error reach to a local minimum
if( error < IK_TOLERANCE ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( abs(error_old - error) < IK_TOLERANCE ){
matcp_c1(dof, q_curr, q_out );
return error;
}
else if( error > error_old ){
matcp_c1(dof, q_old, q_out );
return error_old;
}
matcp_c1(dof, q_curr, q_old );
// solve inverse kinematics
matsvdinvDLS(OJ, 6, dof, 0.01, OJi );
// set weight
for( i=0; i<dof; i++ ){
for( j=0; j<6; j++ ){
OJi[i][j] *= sDH[active_index[i]].weight;
}
}
matnull(6, dof, OJ, OJi, N );
matsub_c1(dof, q_rest, q_curr, q_null );
for(i=0; i<dof; i++ ) q_null[i] *= lamda;
matmul_vec(OJi, dof, 6, ox, q_delta );
matadd_c1(dof, q_delta, q_curr );
matmul_vec( N, dof, dof, q_null, q_delta );
matadd_c1(dof, q_delta, q_curr );
checkVelocityLimit(q_old, q_curr, deltaT);
setJoints(q_curr);
getEndPos(x);
getEndDirAxis(axis, dir);
for( i=0; i<3; i++ ){
ox[i ] = x_dest[i]-x[i];
ox[i+3] = dir_dest[i] - dir[i];
}
error = vnorm(6, ox );
error_old = error;
}
}
void
old_grounded_moving (void)
{
double now, dt;
struct pt newpos;
struct groundtri *gt;
struct vect v1, v2, ctrl_vel;
now = get_secs ();
dt = now - player.lasttime;
ctrl_vel.x = 0;
ctrl_vel.y = 0;
ctrl_vel.z = 0;
if (player.p.z <= ground_height (&player.p)) {
switch (player.moving) {
case BACK:
ctrl_vel.x = -.3 * player.speed * cos (player.theta);
ctrl_vel.y = -.3 * player.speed * sin (player.theta);
break;
case FORW:
ctrl_vel.x = player.speed * cos (player.theta);
ctrl_vel.y = player.speed * sin (player.theta);
break;
case SIDE_Q:
ctrl_vel.x = player.speed
* cos (player.theta + DTOR (90));
ctrl_vel.y = player.speed
* sin (player.theta + DTOR (90));
break;
case SIDE_E:
ctrl_vel.x = player.speed
* cos (player.theta - DTOR (90));
ctrl_vel.y = player.speed
* sin (player.theta - DTOR (90));
break;
case FORW_Q:
ctrl_vel.x = player.speed
* cos (player.theta + DTOR (45));
ctrl_vel.y = player.speed
* sin (player.theta + DTOR (45));
break;
case FORW_E:
ctrl_vel.x = player.speed
* cos (player.theta - DTOR (45));
ctrl_vel.y = player.speed
* sin (player.theta - DTOR (45));
break;
case BACK_Q:
ctrl_vel.x = -.3 * player.speed
* cos (player.theta - DTOR (45));
ctrl_vel.y = -.3 * player.speed
* sin (player.theta - DTOR (45));
break;
case BACK_E:
ctrl_vel.x = -.3 * player.speed
* cos (player.theta + DTOR (45));
ctrl_vel.y = -.3 * player.speed
* sin (player.theta + DTOR (45));
break;
}
}
newpos.x = player.p.x + ctrl_vel.x * dt;
newpos.y = player.p.y + ctrl_vel.y * dt;
newpos.z = player.p.z + ctrl_vel.z * dt;
psub (&player.vel, &newpos, &player.p);
vscal (&player.vel, &player.vel, 1 / dt);
if ((gt = detect_plane (&newpos)) == NULL) {
printf ("Can't find ground, moving player to start position "
"to avoid seg fault\n");
player.p.x = 0;
player.p.y = 0;
player.p.z = ground_height (&player.p);
player.movtype = GROUNDED;
player.loc = detect_plane (&player.p);
return;
}
if (gt->theta < DTOR (50)) {
newpos.z = ground_height (&newpos);
psub (&player.vel, &newpos, &player.p);
vscal (&player.vel, &player.vel, 1 / dt);
player.p = newpos;
} else if (newpos.z > gt->pl.middle.z) {
player.p = newpos;
player.movtype = FALLING;
printf ("switching to falling\n");
} else if (newpos.z <= gt->pl.middle.z) {
if (gt == player.loc) {
printf ("line %d: some sort of bug caused"
" player to be in grounded mode while"
" on steep plane\n", __LINE__);
pt_on_z_plane (&newpos, &newpos, &player.loc->pl);
player.p = newpos;
printf ("shifting player to nearest point on"
" plane with same z\n");
vset (&player.vel, 0, 0, 0);
printf ("killing player velocity\n");
player.movtype = FALLING;
printf ("switching to falling\n");
paused = YES;
printf ("paused game\n");
return;
}
vnorm (&v2, &ctrl_vel);
vcross (&v1, &player.loc->normv, >->normv);
vnorm (&v1, &v1);
if (vdot (&v1, &v2) < 0)
vscal (&v1, &v1, -1);
vscal (&ctrl_vel, &v1, vdot (&ctrl_vel, &v1));
newpos.x = player.p.x + ctrl_vel.x * dt;
newpos.y = player.p.y + ctrl_vel.y * dt;
newpos.z = player.p.z + ctrl_vel.z * dt;
if ((gt = detect_plane (&newpos)) == NULL) {
printf ("can't detect plane, exiting\n");
exit (1);
}
if (gt->theta < DTOR (50)) {
newpos.z = ground_height (&newpos);
psub (&player.vel, &newpos, &player.p);
vscal (&player.vel, &player.vel, 1 / dt);
printf ("bar\n");
player.p = newpos;
} else {
// struct groundtri *first_col, *last_col, *gt2;
// int i;
//PLAYER STARTS MOVING INTO GROUND BEFORE THIS EVER
//HAPPENS
printf ("pausing\n");
paused = YES;
/* i = 0; */
/* first_col = NULL; */
/* last_col = NULL; */
/* for (gt2 = first_groundtri; gt2; gt2 = gt2->next) { */
/* if (detect_collision (&player.p, */
/* &newpos, gt2)) { */
/* if (first_col == NULL) { */
/* first_col = gt2; */
/* } else { */
/* last_col->next = gt2; */
/* } */
/* last_col = gt2; */
/* i++; */
/* } */
/* } */
/* if (first_col == NULL) { */
/* printf ("%s: %s: %d: something weird\n", */
/* __FILE__, __FUNCTION__, __LINE__); */
/* } */
/* printf ("number of collisions: %d\n", i); */
//TRY PROJECTING PLAYER TO LINE OF INTERSECTION
//BETWEEN OLD & NEW PLANES. IF PLAYER IS STILL
//INSIDE ANY PLANES, GRAB THREE PLANES AND GO
//TO INTERSECTION POINT OF THOSE. MIGHT WANT
//TO CHECK FOR OVER THREE PLANES JUST IN CASE
/* printf ("player.z: %g\n", player.p.z); */
/* newpos = player.p; */
/* rnd_escape (&newpos, gt); */
/* printf ("ran rnd_escape\n"); */
/* gt = detect_plane (&newpos); */
/* double b2; */
/* b2 = newpos.y - (gt->pl.b / gt->pl.a) * newpos.x; */
/* newpos.x = -(b2 * gt->pl.a * gt->pl.b */
/* + gt->pl.c * newpos.z * gt->pl.a */
/* + gt->pl.d * gt->pl.a) */
/* / (square (gt->pl.a) + square (gt->pl.b)); */
/* newpos.y = (gt->pl.b / gt->pl.a) * newpos.x + b2; */
/* player.p = newpos; */
/* printf ("shifting player to nearest point on" */
/* " plane with same z\n"); */
/* gt = detect_plane (&player.p); */
/* printf ("re detected plane\n"); */
/* if (gt->theta >= DTOR (50)) { */
/* printf ("rnd_escape put player on slope," */
/* " changing player to falling\n"); */
/* player.movtype = FALLING; */
/* printf ("killing velocity\n"); */
/* vset (&player.vel, 0, 0, 0); */
/* } */
/* printf ("player.z: %g\n", player.p.z); */
}
} else {
printf ("this should never be printed\n");
}
}
/*************************************************************************
Testing Nearest Neighbor Search on uniformly distributed hypercube
NormType: 0, 1, 2
D: space dimension
N: points count
*************************************************************************/
static void testkdtuniform(const ap::real_2d_array& xy,
const int& n,
const int& nx,
const int& ny,
const int& normtype,
bool& kdterrors)
{
double errtol;
ap::integer_1d_array tags;
ap::real_1d_array ptx;
ap::real_1d_array tmpx;
ap::boolean_1d_array tmpb;
kdtree treex;
kdtree treexy;
kdtree treext;
ap::real_2d_array qx;
ap::real_2d_array qxy;
ap::integer_1d_array qtags;
ap::real_1d_array qr;
int kx;
int kxy;
int kt;
int kr;
double eps;
int i;
int j;
int k;
int task;
bool isequal;
double r;
int q;
int qcount;
qcount = 10;
//
// Tol - roundoff error tolerance (for '>=' comparisons)
//
errtol = 100000*ap::machineepsilon;
//
// fill tags
//
tags.setlength(n);
for(i = 0; i <= n-1; i++)
{
tags(i) = i;
}
//
// build trees
//
kdtreebuild(xy, n, nx, 0, normtype, treex);
kdtreebuild(xy, n, nx, ny, normtype, treexy);
kdtreebuildtagged(xy, tags, n, nx, 0, normtype, treext);
//
// allocate arrays
//
tmpx.setlength(nx);
tmpb.setlength(n);
qx.setlength(n, nx);
qxy.setlength(n, nx+ny);
qtags.setlength(n);
qr.setlength(n);
ptx.setlength(nx);
//
// test general K-NN queries (with self-matches):
// * compare results from different trees (must be equal) and
// check that correct (value,tag) pairs are returned
// * test results from XT tree - let R be radius of query result.
// then all points not in result must be not closer than R.
//
for(q = 1; q <= qcount; q++)
{
//
// Select K: 1..N
//
if( ap::fp_greater(ap::randomreal(),0.5) )
{
k = 1+ap::randominteger(n);
}
else
{
k = 1;
}
//
// Select point (either one of the points, or random)
//
if( ap::fp_greater(ap::randomreal(),0.5) )
{
i = ap::randominteger(n);
ap::vmove(&ptx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
}
else
{
for(i = 0; i <= nx-1; i++)
{
ptx(i) = 2*ap::randomreal()-1;
}
}
//
// Test:
// * consistency of results from different queries
// * points in query are IN the R-sphere (or at the boundary),
// and points not in query are outside of the R-sphere (or at the boundary)
// * distances are correct and are ordered
//
kx = kdtreequeryknn(treex, ptx, k, true);
kxy = kdtreequeryknn(treexy, ptx, k, true);
kt = kdtreequeryknn(treext, ptx, k, true);
if( kx!=k||kxy!=k||kt!=k )
{
kdterrors = true;
return;
}
kx = 0;
kxy = 0;
kt = 0;
kdtreequeryresultsx(treex, qx, kx);
kdtreequeryresultsxy(treexy, qxy, kxy);
kdtreequeryresultstags(treext, qtags, kt);
kdtreequeryresultsdistances(treext, qr, kr);
if( kx!=k||kxy!=k||kt!=k||kr!=k )
{
kdterrors = true;
return;
}
kdterrors = kdterrors||kdtresultsdifferent(xy, n, qx, qxy, qtags, k, nx, ny);
for(i = 0; i <= n-1; i++)
{
tmpb(i) = true;
}
r = 0;
for(i = 0; i <= k-1; i++)
{
tmpb(qtags(i)) = false;
ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
ap::vsub(&tmpx(0), 1, &qx(i, 0), 1, ap::vlen(0,nx-1));
r = ap::maxreal(r, vnorm(tmpx, nx, normtype));
}
for(i = 0; i <= n-1; i++)
{
if( tmpb(i) )
{
ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
ap::vsub(&tmpx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
kdterrors = kdterrors||ap::fp_less(vnorm(tmpx, nx, normtype),r*(1-errtol));
}
}
for(i = 0; i <= k-2; i++)
{
kdterrors = kdterrors||ap::fp_greater(qr(i),qr(i+1));
}
for(i = 0; i <= k-1; i++)
{
ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
ap::vsub(&tmpx(0), 1, &xy(qtags(i), 0), 1, ap::vlen(0,nx-1));
kdterrors = kdterrors||ap::fp_greater(fabs(vnorm(tmpx, nx, normtype)-qr(i)),errtol);
}
}
//
// test general approximate K-NN queries (with self-matches):
// * compare results from different trees (must be equal) and
// check that correct (value,tag) pairs are returned
// * test results from XT tree - let R be radius of query result.
// then all points not in result must be not closer than R/(1+Eps).
//
for(q = 1; q <= qcount; q++)
{
//
// Select K: 1..N
//
if( ap::fp_greater(ap::randomreal(),0.5) )
{
k = 1+ap::randominteger(n);
}
else
{
k = 1;
}
//
// Select Eps
//
eps = 0.5+ap::randomreal();
//
// Select point (either one of the points, or random)
//
if( ap::fp_greater(ap::randomreal(),0.5) )
{
i = ap::randominteger(n);
ap::vmove(&ptx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
}
else
{
for(i = 0; i <= nx-1; i++)
{
ptx(i) = 2*ap::randomreal()-1;
}
}
//
// Test:
// * consistency of results from different queries
// * points in query are IN the R-sphere (or at the boundary),
// and points not in query are outside of the R-sphere (or at the boundary)
// * distances are correct and are ordered
//
kx = kdtreequeryaknn(treex, ptx, k, true, eps);
kxy = kdtreequeryaknn(treexy, ptx, k, true, eps);
kt = kdtreequeryaknn(treext, ptx, k, true, eps);
if( kx!=k||kxy!=k||kt!=k )
{
kdterrors = true;
return;
}
kx = 0;
kxy = 0;
kt = 0;
kdtreequeryresultsx(treex, qx, kx);
kdtreequeryresultsxy(treexy, qxy, kxy);
kdtreequeryresultstags(treext, qtags, kt);
kdtreequeryresultsdistances(treext, qr, kr);
if( kx!=k||kxy!=k||kt!=k||kr!=k )
{
kdterrors = true;
return;
}
kdterrors = kdterrors||kdtresultsdifferent(xy, n, qx, qxy, qtags, k, nx, ny);
for(i = 0; i <= n-1; i++)
{
tmpb(i) = true;
}
r = 0;
for(i = 0; i <= k-1; i++)
{
tmpb(qtags(i)) = false;
ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
ap::vsub(&tmpx(0), 1, &qx(i, 0), 1, ap::vlen(0,nx-1));
r = ap::maxreal(r, vnorm(tmpx, nx, normtype));
}
for(i = 0; i <= n-1; i++)
{
if( tmpb(i) )
{
ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
ap::vsub(&tmpx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
kdterrors = kdterrors||ap::fp_less(vnorm(tmpx, nx, normtype),r*(1-errtol)/(1+eps));
}
}
for(i = 0; i <= k-2; i++)
{
kdterrors = kdterrors||ap::fp_greater(qr(i),qr(i+1));
}
for(i = 0; i <= k-1; i++)
{
ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
ap::vsub(&tmpx(0), 1, &xy(qtags(i), 0), 1, ap::vlen(0,nx-1));
kdterrors = kdterrors||ap::fp_greater(fabs(vnorm(tmpx, nx, normtype)-qr(i)),errtol);
}
}
//
// test general R-NN queries (with self-matches):
// * compare results from different trees (must be equal) and
// check that correct (value,tag) pairs are returned
// * test results from XT tree - let R be radius of query result.
// then all points not in result must be not closer than R.
//
for(q = 1; q <= qcount; q++)
{
//
// Select R
//
if( ap::fp_greater(ap::randomreal(),0.3) )
{
r = ap::maxreal(ap::randomreal(), ap::machineepsilon);
}
else
{
r = ap::machineepsilon;
}
//
// Select point (either one of the points, or random)
//
if( ap::fp_greater(ap::randomreal(),0.5) )
{
i = ap::randominteger(n);
ap::vmove(&ptx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
}
else
{
for(i = 0; i <= nx-1; i++)
{
ptx(i) = 2*ap::randomreal()-1;
}
}
//
// Test:
// * consistency of results from different queries
// * points in query are IN the R-sphere (or at the boundary),
// and points not in query are outside of the R-sphere (or at the boundary)
// * distances are correct and are ordered
//
kx = kdtreequeryrnn(treex, ptx, r, true);
kxy = kdtreequeryrnn(treexy, ptx, r, true);
kt = kdtreequeryrnn(treext, ptx, r, true);
if( kxy!=kx||kt!=kx )
{
kdterrors = true;
return;
}
kx = 0;
kxy = 0;
kt = 0;
kdtreequeryresultsx(treex, qx, kx);
kdtreequeryresultsxy(treexy, qxy, kxy);
kdtreequeryresultstags(treext, qtags, kt);
kdtreequeryresultsdistances(treext, qr, kr);
if( kxy!=kx||kt!=kx||kr!=kx )
{
kdterrors = true;
return;
}
kdterrors = kdterrors||kdtresultsdifferent(xy, n, qx, qxy, qtags, kx, nx, ny);
for(i = 0; i <= n-1; i++)
{
tmpb(i) = true;
}
for(i = 0; i <= kx-1; i++)
{
tmpb(qtags(i)) = false;
}
for(i = 0; i <= n-1; i++)
{
ap::vmove(&tmpx(0), 1, &ptx(0), 1, ap::vlen(0,nx-1));
ap::vsub(&tmpx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
if( tmpb(i) )
{
kdterrors = kdterrors||ap::fp_less(vnorm(tmpx, nx, normtype),r*(1-errtol));
}
else
{
kdterrors = kdterrors||ap::fp_greater(vnorm(tmpx, nx, normtype),r*(1+errtol));
}
}
for(i = 0; i <= kx-2; i++)
{
kdterrors = kdterrors||ap::fp_greater(qr(i),qr(i+1));
}
}
//
// Test self-matching:
// * self-match - nearest neighbor of each point in XY is the point itself
// * no self-match - nearest neighbor is NOT the point itself
//
if( n>1 )
{
//
// test for N=1 have non-general form, but it is not really needed
//
for(task = 0; task <= 1; task++)
{
for(i = 0; i <= n-1; i++)
{
ap::vmove(&ptx(0), 1, &xy(i, 0), 1, ap::vlen(0,nx-1));
kx = kdtreequeryknn(treex, ptx, 1, task==0);
kdtreequeryresultsx(treex, qx, kx);
if( kx!=1 )
{
kdterrors = true;
return;
}
isequal = true;
for(j = 0; j <= nx-1; j++)
{
isequal = isequal&&ap::fp_eq(qx(0,j),ptx(j));
}
if( task==0 )
{
kdterrors = kdterrors||!isequal;
}
else
{
kdterrors = kdterrors||isequal;
}
}
}
}
}
static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
KX_Obstacles& obstacles, float levelHeight, const float vmax,
const float* spos, const float cs, const int nspos, float* res,
float maxToi, float velWeight, float curVelWeight, float sideWeight,
float toiWeight)
{
vset(res, 0,0);
const float ivmax = 1.0f / vmax;
float adir[2], adist;
vcpy(adir, activeObst->pvel);
if (vlen(adir) > 0.01f)
vnorm(adir);
else
vset(adir,0,0);
float activeObstPos[2];
vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y());
adist = vdot(adir, activeObstPos);
float minPenalty = FLT_MAX;
for (int n = 0; n < nspos; ++n)
{
float vcand[2];
vcpy(vcand, &spos[n*2]);
// Find min time of impact and exit amongst all obstacles.
float tmin = maxToi;
float side = 0;
int nside = 0;
for (int i = 0; i < obstacles.size(); ++i)
{
KX_Obstacle* ob = obstacles[i];
bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight);
if (!res)
continue;
float htmin, htmax;
if (ob->m_shape==KX_OBSTACLE_CIRCLE)
{
float vab[2];
// Moving, use RVO
vscale(vab, vcand, 2);
vsub(vab, vab, activeObst->vel);
vsub(vab, vab, ob->vel);
// Side
// NOTE: dp, and dv are constant over the whole calculation,
// they can be precomputed per object.
const float* pa = activeObstPos;
float pb[2];
vset(pb, ob->m_pos.x(), ob->m_pos.y());
const float orig[2] = {0,0};
float dp[2],dv[2],np[2];
vsub(dp,pb,pa);
vnorm(dp);
vsub(dv,ob->dvel, activeObst->dvel);
const float a = triarea(orig, dp,dv);
if (a < 0.01f)
{
np[0] = -dp[1];
np[1] = dp[0];
}
else
{
np[0] = dp[1];
np[1] = -dp[0];
}
side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f);
nside++;
if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad,
htmin, htmax))
continue;
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
// Avoid more when overlapped.
htmin = -htmin * 0.5f;
}
}
else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
{
MT_Point3 p1 = ob->m_pos;
MT_Point3 p2 = ob->m_pos2;
//apply world transform
if (ob->m_type == KX_OBSTACLE_NAV_MESH)
{
KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
p1 = navmeshobj->TransformToWorldCoords(p1);
p2 = navmeshobj->TransformToWorldCoords(p2);
}
float p[2], q[2];
vset(p, p1.x(), p1.y());
vset(q, p2.x(), p2.y());
// NOTE: the segments are assumed to come from a navmesh which is shrunken by
// the agent radius, hence the use of really small radius.
// This can be handle more efficiently by using seg-seg test instead.
// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
const float r = 0.01f; // agent->rad
if (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad))
{
float sdir[2], snorm[2];
vsub(sdir, q, p);
snorm[0] = sdir[1];
snorm[1] = -sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (vdot(snorm, vcand) < 0.0f)
continue;
// Else immediate collision.
htmin = 0.0f;
htmax = 10.0f;
}
else
{
if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax))
continue;
}
// Avoid less when facing walls.
htmin *= 2.0f;
}
if (htmin >= 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
tmin = htmin;
}
}
// Normalize side bias, to prevent it dominating too much.
if (nside)
side /= nside;
const float vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax);
const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax);
const float spen = sideWeight * side;
const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));
const float penalty = vpen + vcpen + spen + tpen;
if (penalty < minPenalty)
{
minPenalty = penalty;
vcpy(res, vcand);
}
}
}
Symbolhandle rangen(Symbolhandle list)
{
Symbolhandle result = (Symbolhandle) 0, symhN, symhParam1, symhParam2;
long sampleSize, lengthParam1, lengthParam2;
long op, verbose = 1;
long nargs = NARGS(list), margs;
char *what1, *what2;
char *badarg = "argument %ld (%s) to %s()";
char *badParam =
"ERROR: argument %ld to %s() (%s) not REAL scalar or vector";
char *hasMissing =
"ERROR: argument %ld to %s() (%s) has 1 or more MISSING values";
char *badLength =
"ERROR: length of vector argument %ld to %s() (%s) differs from sample size";
WHERE("rangen");
/* generate a list of random numbers */
if (strcmp(FUNCNAME, "rnorm") == 0)
{
op = IRNORM;
}
else if (strcmp(FUNCNAME, "rpoi") == 0)
{
op = IRPOI;
what1 = "mean";
}
else if (strcmp(FUNCNAME, "rbin") == 0)
{
op = IRBINOM;
what1 = "n";
what2 = "p";
}
else
{
op = IRUNI;
}
margs = (op == IRBINOM) ? 3 : ((op == IRPOI) ? 2 : 1);
OUTSTR[0] = '\0';
if (nargs != margs)
{
badNargs(FUNCNAME, margs);
goto errorExit;
}
symhN = COMPVALUE(list,0);
if (!isInteger(symhN, POSITIVEVALUE))
{
char outstr[30];
sprintf(outstr, badarg, 1L, "sample size", FUNCNAME);
notPositiveInteger(outstr);
goto errorExit;
} /*if (!isInteger(symhN, POSITIVEVALUE))*/
sampleSize = (long) DATAVALUE(symhN,0);
if (margs > 1)
{
symhParam1 = COMPVALUE(list, 1);
if (!argOK(symhParam1, 0, 2))
{
goto errorExit;
}
lengthParam1 = symbolSize(symhParam1);
if (TYPE(symhParam1) != REAL || !isVector(symhParam1))
{
sprintf(OUTSTR, badParam, 2L, what1, FUNCNAME);
}
else if (anyMissing(symhParam1))
{
sprintf(OUTSTR, hasMissing, 2L, what1, FUNCNAME);
}
else if (lengthParam1 > 1 && lengthParam1 != sampleSize)
{
sprintf(OUTSTR, badLength, 2L, what1, FUNCNAME);
}
else if (margs > 2)
{
symhParam2 = COMPVALUE(list, 2);
if (!argOK(symhParam2, 0, 3))
{
goto errorExit;
}
lengthParam2 = symbolSize(symhParam2);
if (TYPE(symhParam2) != REAL || !isVector(symhParam2))
{
sprintf(OUTSTR, badParam, 3L, what2, FUNCNAME);
}
else if (anyMissing(symhParam2))
{
sprintf(OUTSTR, hasMissing, 3L, what2, FUNCNAME);
}
else if (lengthParam2 > 1 && lengthParam2 != sampleSize)
{
sprintf(OUTSTR, badLength, 3L, what2, FUNCNAME);
}
}
if (*OUTSTR)
{
goto errorExit;
}
if (op == IRPOI)
{
/*rpoi(lambda)*/
if (doubleMin(DATAPTR(symhParam1), lengthParam1) < 0.0)
{
sprintf(OUTSTR,
"ERROR: argument 2 (%s) to %s() has negative element",
what1, FUNCNAME);
}
}
else if (op == IRBINOM)
{
/*rbinom(samplesize,n,p)*/
long i;
double *n = DATAPTR(symhParam1);
for (i = 0; i < lengthParam1; i++)
{
if (n[i] < 1 || n[i] != floor(n[i]))
{
sprintf(OUTSTR,
"ERROR: not all elements of argument 2 (%s) to %s() are positive integers",
what1, FUNCNAME);
goto errorExit;
}
} /*for (i = 0; i < lengthParam1; i++)*/
if (doubleMin(DATAPTR(symhParam2), lengthParam2) < 0.0 ||
doubleMax(DATAPTR(symhParam2), lengthParam2) > 1.0)
{
sprintf(OUTSTR,
"ERROR: argument 3 (%s) to %s() has value < 0 or > 1",
what2, FUNCNAME);
}
} /*else if (op == IRBINOM)*/
if (*OUTSTR)
{
goto errorExit;
}
} /*if (margs > 1)*/
if (Rands1 == 0 && Rands2 == 0)
{
randomSeed(verbose);
} /*if (Rands1 == 0 && Rands2 == 0)*/
result = RInstall(SCRATCH, sampleSize);
if (result != (Symbolhandle) 0)
{
switch (op)
{
case IRUNI:
vuni(sampleSize, DATAPTR(result));
break;
case IRNORM:
vnorm(sampleSize, DATAPTR(result));
break;
case IRPOI:
vpoi(sampleSize, DATAPTR(symhParam1), lengthParam1,
DATAPTR(result));
break;
case IRBINOM:
vbinom(sampleSize, DATAPTR(symhParam1), lengthParam1,
DATAPTR(symhParam2), lengthParam2, DATAPTR(result));
break;
} /*switch (op)*/
} /*if (result != (Symbolhandle) 0)*/
return (result);
errorExit:
putErrorOUTSTR();
return (0);
} /*rangen()*/
void
define_plane (double x1, double y1, double z1,
double x2, double y2, double z2,
double x3, double y3, double z3,
double normx, double normy, double normz)
{
struct groundtri *gt;
struct vect v1, v2;
gt = xcalloc (1, sizeof *gt);
if (first_groundtri == NULL) {
first_groundtri = gt;
} else {
last_groundtri->next = gt;
}
last_groundtri = gt;
gt->pl.a = normx;
gt->pl.b = normy;
gt->pl.c = normz;
gt->pl.d = -(gt->pl.a * x1 + gt->pl.b * y1 + gt->pl.c * z1);
gt->normv.x = normx;
gt->normv.y = normy;
gt->normv.z = normz;
vnorm (>->normv, >->normv);
gt->p1.x = x1;
gt->p1.y = y1;
gt->p1.z = z1;
gt->p2.x = x2;
gt->p2.y = y2;
gt->p2.z = z2;
gt->p3.x = x3;
gt->p3.y = y3;
gt->p3.z = z3;
gt->pl.middle.x = (x1 + x2 + x3) / 3;
gt->pl.middle.y = (y1 + y2 + y3) / 3;
gt->pl.middle.z = (z1 + z2 + z3) / 3;
if (fabs (gt->pl.c) < 1e-6) {
gt->downhill.x = 0;
gt->downhill.y = 0;
gt->downhill.z = -1;
} else if (hypot (gt->pl.a, gt->pl.b) < 1e-6) {
gt->downhill.x = 1;
gt->downhill.y = 0;
gt->downhill.z = 0;
} else {
gt->downhill.x = gt->pl.a;
gt->downhill.y = gt->pl.b;
gt->downhill.z = (-gt->pl.a * gt->pl.a - gt->pl.b * gt->pl.b - gt->pl.d)
/ gt->pl.c;
}
vnorm (>->downhill, >->downhill);
vscal (&v1, >->downhill, -1);
if (v1.x == 0 && v1.y == 0) {
v2.x = 1;
v2.y = 1;
v2.z = 0;
} else {
v2.x = v1.x;
v2.y = v1.y;
v2.z = 0;
}
gt->theta = acos (vdot (&v1, &v2));
gt->gndnum = gndcounter;
gndcounter++;
}
void
DoFlares(GLfloat from[3], GLfloat at[3], GLfloat light[3], GLfloat near_clip)
{
GLfloat view_dir[3], tmp[3], light_dir[3], position[3], dx[3], dy[3],
center[3], axis[3], sx[3], sy[3], dot, global_scale = 1.5;
GLuint bound_to = 0;
int i;
/* view_dir = normalize(at-from) */
vdiff(view_dir, at, from);
vnorm(view_dir);
/* center = from + near_clip * view_dir */
vscale(tmp, view_dir, near_clip);
vadd(center, from, tmp);
/* light_dir = normalize(light-from) */
vdiff(light_dir, light, from);
vnorm(light_dir);
/* light = from + dot(light,view_dir)*near_clip*light_dir */
dot = vdot(light_dir, view_dir);
vscale(tmp, light_dir, near_clip / dot);
vadd(light, from, light_dir);
/* axis = light - center */
vdiff(axis, light, center);
vcopy(dx, axis);
/* dx = normalize(axis) */
vnorm(dx);
/* dy = cross(dx,view_dir) */
vcross(dy, dx, view_dir);
glDisable(GL_DEPTH_TEST);
glDisable(GL_DITHER);
glEnable(GL_BLEND);
glBlendFunc(GL_ONE, GL_ONE);
for (i = 0; i < num_flares; i++) {
vscale(sx, dx, flare[i].scale * global_scale);
vscale(sy, dy, flare[i].scale * global_scale);
glColor3fv(flare[i].color);
/* Note logic below to eliminate duplicate texture binds. */
if (flare[i].type < 0) {
if (bound_to)
glEnd();
glBindTexture(GL_TEXTURE_2D, shineTex[shine_tic]);
bound_to = shineTex[shine_tic];
shine_tic = (shine_tic + 1) % 10;
glBegin(GL_QUADS);
} else {
if (bound_to != flareTex[flare[i].type]) {
glEnd();
glBindTexture(GL_TEXTURE_2D, flareTex[flare[i].type]);
bound_to = flareTex[flare[i].type];
glBegin(GL_QUADS);
}
}
/* position = center + flare[i].loc * axis */
vscale(tmp, axis, flare[i].loc);
vadd(position, center, tmp);
glTexCoord2f(0.0, 0.0);
vadd(tmp, position, sx);
vadd(tmp, tmp, sy);
glVertex3fv(tmp);
glTexCoord2f(1.0, 0.0);
vdiff(tmp, position, sx);
vadd(tmp, tmp, sy);
glVertex3fv(tmp);
glTexCoord2f(1.0, 1.0);
vdiff(tmp, position, sx);
vdiff(tmp, tmp, sy);
glVertex3fv(tmp);
glTexCoord2f(0.0, 1.0);
vadd(tmp, position, sx);
vdiff(tmp, tmp, sy);
glVertex3fv(tmp);
}
glEnd();
}