void KoulesSimulator::elasticCollision(unsigned int i, unsigned int j)
{
double *a = &qcur_[ind(i)], *b = &qcur_[ind(j)];
double d[2] = { b[0] - a[0], b[1] - a[1] };
normalize2(d);
double va = dot2(a + 2, d), vb = dot2(b + 2, d);
if (va - vb <= 0.)
return;
double ma = si_->getStateSpace()->as<KoulesStateSpace>()->getMass(i);
double mb = si_->getStateSpace()->as<KoulesStateSpace>()->getMass(j);
double vap = (va * (ma - mb) + 2. * mb * vb) / (ma + mb);
double vbp = (vb * (mb - ma) + 2. * ma * va) / (ma + mb);
double amag = vap - va, bmag = vbp - vb;
if (std::abs(amag) < eps)
amag = amag < 0. ? -eps : eps;
if (std::abs(bmag) < eps)
bmag = bmag < 0. ? -eps : eps;
#ifndef NDEBUG
double px = ma * a[2] + mb * b[2], py = ma * a[3] + mb * b[3];
double k = ma * (a[2] * a[2] + a[3] * a[3]) + mb * (b[2] * b[2] + b[3] * b[3]);
#endif
vadd2(a + 2, amag, d);
vadd2(b + 2, bmag, d);
#ifndef NDEBUG
// preservation of momemtum
assert(std::abs(ma * a[2] + mb * b[2] - px) < 1e-6);
assert(std::abs(ma * a[3] + mb * b[3] - py) < 1e-6);
// preservation of kinetic energy
assert(std::abs(ma * (a[2] * a[2] + a[3] * a[3]) + mb * (b[2] * b[2] + b[3] * b[3]) - k) < 1e-6);
#endif
}
matrix2_2 matMul_22x22(matrix2_2 a, matrix2_2 b)
{
matrix2_2 result;
result.one.a = dot2(a.one, vect2Make(b.one.a, b.two.a));
result.one.b = dot2(a.one, vect2Make(b.one.b, b.two.b));
result.two.a = dot2(a.two, vect2Make(b.one.a, b.two.a));
result.two.b = dot2(a.two, vect2Make(b.one.b, b.two.b));
return result;
}
/* ------------------
movement control
------------------ - */
void Movement(BYTE code, BOOL4 value)
{
FnCharacter actor;
actor.ID(actorID);
bool up = FyCheckHotKeyStatus(FY_UP) || FyCheckHotKeyStatus('W'),
down = FyCheckHotKeyStatus(FY_DOWN) || FyCheckHotKeyStatus('S'),
left = FyCheckHotKeyStatus(FY_LEFT) || FyCheckHotKeyStatus('A'),
right = FyCheckHotKeyStatus(FY_RIGHT) || FyCheckHotKeyStatus('D'),
act = (frame - frameattack + 1000) % 1000 >= attacktime;
if (code == 'J'&&value == TRUE)
{
if ((frame - frameattack + 1000) % 1000> attacktime)
{
actor.SetCurrentAction(NULL, 0, attackID, 5.0f);
actor.Play(ONCE, 0.0f, FALSE, TRUE);
frameattack = frame;
FnCharacter npc1, npc2;
npc1.ID(npc1ID);
npc2.ID(npc2ID);
float apos[3], npos[3], vector[2], fDir[3], rDir[3];
actor.GetDirection(fDir, NULL);
actor.GetPosition(apos);
npc1.GetPosition(npos);
vector[0] = npos[0] - apos[0];
vector[1] = npos[1] - apos[1];
rDir[0] = fDir[1]; rDir[1] = -fDir[0];
if ((HP1>0) && (dot2(vector, fDir) / norm2(fDir)>0.0f) && (dot2(vector, fDir) / norm2(fDir) < 150.0f) && (abs(dot2(vector, rDir)) / norm2(rDir) < 20.0f))
{
framedamage1 = frame + 10;
HP1--;
}
npc2.GetPosition(npos);
vector[0] = npos[0] - apos[0];
vector[1] = npos[1] - apos[1];
rDir[0] = fDir[1]; rDir[1] = -fDir[0];
if ((HP2>0) && (dot2(vector, fDir) / norm2(fDir)>0.0f) && (dot2(vector, fDir) / norm2(fDir) < 150.0f) && (abs(dot2(vector, rDir)) / norm2(rDir) < 20.0f))
{
framedamage2 = frame + 10;
HP2--;
}
}
}
else if (((up || down) && !(up&&down)) || ((left || right) && !(left&&right)))
{
curPoseID = runID;
if (act)
actor.SetCurrentAction(NULL, 0, curPoseID, 5.0f);
}
else
{
curPoseID = idleID;
if (act)
actor.SetCurrentAction(NULL, 0, curPoseID, 5.0f);
}
}
float simplexRawNoise2( const float x, const float y ) {
float n0, n1, n2;
float F2 = 0.5 * (sqrtf(3.0) - 1.0);
float s = (x + y) * F2;
int i = fastfloor( x + s );
int j = fastfloor( y + s );
float G2 = (3.0 - sqrtf(3.0)) / 6.0;
float t = (i + j) * G2;
float X0 = i-t;
float Y0 = j-t;
float x0 = x-X0;
float y0 = y-Y0;
int i1, j1;
if(x0>y0) {i1=1; j1=0;}
else {i1=0; j1=1;}
float x1 = x0 - i1 + G2;
float y1 = y0 - j1 + G2;
float x2 = x0 - 1.0 + 2.0 * G2;
float y2 = y0 - 1.0 + 2.0 * G2;
int ii = i & 255;
int jj = j & 255;
int gi0 = perm[ii+perm[jj]] % 12;
int gi1 = perm[ii+i1+perm[jj+j1]] % 12;
int gi2 = perm[ii+1+perm[jj+1]] % 12;
float t0 = 0.5 - x0*x0-y0*y0;
if(t0<0) n0 = 0.0;
else {
t0 *= t0;
n0 = t0 * t0 * dot2(grad3[gi0], x0, y0);
}
float t1 = 0.5 - x1*x1-y1*y1;
if(t1<0) n1 = 0.0;
else {
t1 *= t1;
n1 = t1 * t1 * dot2(grad3[gi1], x1, y1);
}
float t2 = 0.5 - x2*x2-y2*y2;
if(t2<0) n2 = 0.0;
else {
t2 *= t2;
n2 = t2 * t2 * dot2(grad3[gi2], x2, y2);
}
return 70.0 * (n0 + n1 + n2);
}
static int luaB_dot (lua_State *L) {
float x1, y1, z1, w1;
float x2, y2, z2, w2;
if (lua_gettop(L) != 2) luaL_error(L, "Invalid params, try dot(v,v)");
if (lua_isvector4(L,1)) {
lua_checkvector4(L, 1, &x1, &y1, &z1, &w1);
lua_checkvector4(L, 2, &x2, &y2, &z2, &w2);
lua_pushnumber(L,dot4(x1,y1,z1,w1, x2,y2,z2,w2));
} else if (lua_isvector3(L,1)) {
lua_checkvector3(L, 1, &x1, &y1, &z1);
lua_checkvector3(L, 2, &x2, &y2, &z2);
lua_pushnumber(L,dot3(x1,y1,z1, x2,y2,z2));
} else if (lua_isvector2(L,1)) {
lua_checkvector2(L, 1, &x1, &y1);
lua_checkvector2(L, 2, &x2, &y2);
lua_pushnumber(L,dot2(x1,y1, x2,y2));
}
return 1;
}
float simplexnoise( float xin, float yin )
{
float n0, n1, n2; // Noise contributions from the three corners
// Skew the input space to determine which simplex cell we're in
const float F2 = 0.5 * ( sqrt( 3.0 ) - 1.0 );
float s = ( xin + yin ) * F2; // Hairy factor for 2D
int i = floor( xin + s );
int j = floor( yin + s );
const float G2 = ( 3.0 - sqrt( 3.0 ) ) / 6.0;
float t = ( i + j ) * G2;
float X0 = i - t; // Unskew the cell origin back to (x,y) space
float Y0 = j - t;
float x0 = xin - X0; // The x,y distances from the cell origin
float y0 = yin - Y0;
// For the 2D case, the simplex shape is an equilateral triangle.
// Determine which simplex we are in.
int i1, j1; // Offsets for second (middle) corner of simplex in (i,j) coords
if( x0 > y0 )
{
i1 = 1;
j1 = 0;
} // lower triangle, XY order: (0,0)->(1,0)->(1,1)
else
{
i1 = 0;
j1 = 1;
} // upper triangle, YX order: (0,0)->(0,1)->(1,1)
// A step of (1,0) in (i,j) means a step of (1-c,-c) in (x,y), and
// a step of (0,1) in (i,j) means a step of (-c,1-c) in (x,y), where
// c = (3-sqrt(3))/6
float x1 = x0 - i1 + G2; // Offsets for middle corner in (x,y) unskewed coords
float y1 = y0 - j1 + G2;
float x2 = x0 - 1.0 + 2.0 * G2; // Offsets for last corner in (x,y) unskewed coords
float y2 = y0 - 1.0 + 2.0 * G2;
// Work out the hashed gradient indices of the three simplex corners
int ii = i & 255;
int jj = j & 255;
int gi0 = perm[ii + perm[jj]] % 12;
int gi1 = perm[ii + i1 + perm[jj + j1]] % 12;
int gi2 = perm[ii + 1 + perm[jj + 1]] % 12;
// Calculate the contribution from the three corners
float t0 = 0.5 - x0 * x0 - y0 * y0;
if( t0 < 0 )
n0 = 0.0;
else
{
t0 *= t0;
n0 = t0 * t0 * dot2( grad3[gi0], x0, y0 ); // (x,y) of grad3 used for 2D gradient
}
float t1 = 0.5 - x1 * x1 - y1 * y1;
if( t1 < 0 )
n1 = 0.0;
else
{
t1 *= t1;
n1 = t1 * t1 * dot2( grad3[gi1], x1, y1 );
}
float t2 = 0.5 - x2 * x2 - y2 * y2;
if( t2 < 0 )
n2 = 0.0;
else
{
t2 *= t2;
n2 = t2 * t2 * dot2( grad3[gi2], x2, y2 );
}
// Add contributions from each corner to get the final noise value.
// The result is scaled to return values in the interval [-1,1].
return 70.0 * ( n0 + n1 + n2 );
}
float vec::magnitude2() const { return sqrtf(dot2(*this)); }
/*
* return the minimum on the projective line
*
*/
int lp_base_case(FLOAT halves[][2], /* halves --- half lines */
int m, /* m --- terminal marker */
FLOAT n_vec[2], /* n_vec --- numerator funciton */
FLOAT d_vec[2], /* d_vec --- denominator function */
FLOAT opt[2], /* opt --- optimum */
int next[],
int prev[]) /* next, prev ---
double linked list of indices */
{
FLOAT cw_vec[2], ccw_vec[2];
#ifdef CHECK
FLOAT d_cw;
int i;
#endif
int status, degen;
FLOAT ab;
/* find the feasible region of the line */
status = wedge(halves,m,next,prev,cw_vec,ccw_vec,°en);
if(status==INFEASIBLE) return(status);
/* no non-trivial constraints one the plane: return the unconstrained
** optimum */
if(status==UNBOUNDED) {
return(lp_no_constraints(1,n_vec,d_vec,opt));
}
ab = ABS(cross2(n_vec,d_vec));
if(ab < 2*EPS*EPS) {
if(dot2(n_vec,n_vec) < 2*EPS*EPS ||
dot2(d_vec,d_vec) > 2*EPS*EPS) {
/* numerator is zero or numerator and denominator are linearly dependent */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
status = AMBIGUOUS;
} else {
/* numerator is non-zero and denominator is zero
** minimize linear functional on circle */
if(!degen && cross2(cw_vec,n_vec) <= 0.0 &&
cross2(n_vec,ccw_vec) <= 0.0 ) {
/* optimum is in interior of feasible region */
opt[0] = -n_vec[0];
opt[1] = -n_vec[1];
} else if(dot2(n_vec,cw_vec) > dot2(n_vec,ccw_vec) ) {
/* optimum is at counter-clockwise boundary */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
} else {
/* optimum is at clockwise boundary */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
status = MINIMUM;
}
} else {
/* niether numerator nor denominator is zero */
lp_min_lin_rat(degen,cw_vec,ccw_vec,n_vec,d_vec,opt);
status = MINIMUM;
}
#ifdef CHECK
for(i=0; i!=m; i=next[i]) {
d_cw = dot2(opt,halves[i]);
if(d_cw < -2*EPS) {
printf("error at base level\n");
exit(1);
}
}
#endif
return(status);
}
int wedge(FLOAT halves[][2],
int m,
int next[],
int prev[],
FLOAT cw_vec[],
FLOAT ccw_vec[],
int *degen)
{
int i;
FLOAT d_cw, d_ccw;
int offensive;
*degen = 0;
for(i=0;i!=m;i = next[i]) {
if(!unit2(halves[i],ccw_vec,EPS)) {
/* clock-wise */
cw_vec[0] = ccw_vec[1];
cw_vec[1] = -ccw_vec[0];
/* counter-clockwise */
ccw_vec[0] = -cw_vec[0];
ccw_vec[1] = -cw_vec[1];
break;
}
}
if(i==m) return(UNBOUNDED);
i = 0;
while(i!=m) {
offensive = 0;
d_cw = dot2(cw_vec,halves[i]);
d_ccw = dot2(ccw_vec,halves[i]);
if(d_ccw >= 2*EPS) {
if(d_cw <= -2*EPS) {
cw_vec[0] = halves[i][1];
cw_vec[1] = -halves[i][0];
(void)unit2(cw_vec,cw_vec,EPS);
offensive = 1;
}
} else if(d_cw >= 2*EPS) {
if(d_ccw <= -2*EPS) {
ccw_vec[0] = -halves[i][1];
ccw_vec[1] = halves[i][0];
(void)unit2(ccw_vec,ccw_vec,EPS);
offensive = 1;
}
} else if(d_ccw <= -2*EPS && d_cw <= -2*EPS) {
return(INFEASIBLE);
} else if((d_cw <= -2*EPS) ||
(d_ccw <= -2*EPS) ||
(cross2(cw_vec,halves[i]) < 0.0)) {
/* degenerate */
if(d_cw <= -2*EPS) {
(void)unit2(ccw_vec,cw_vec,EPS);
} else if(d_ccw <= -2*EPS) {
(void)unit2(cw_vec,ccw_vec,EPS);
}
*degen = 1;
offensive = 1;
}
/* place this offensive plane in second place */
if(offensive) i = move_to_front(i,next,prev);
i = next[i];
if(*degen) break;
}
if(*degen) {
while(i!=m) {
d_cw = dot2(cw_vec,halves[i]);
d_ccw = dot2(ccw_vec,halves[i]);
if(d_cw < -2*EPS) {
if(d_ccw < -2*EPS) {
return(INFEASIBLE);
} else {
cw_vec[0] = ccw_vec[0];
cw_vec[1] = ccw_vec[1];
}
} else if(d_ccw < -2*EPS) {
ccw_vec[0] = cw_vec[0];
ccw_vec[1] = cw_vec[1];
}
i = next[i];
}
}
return(MINIMUM);
}
void lp_min_lin_rat(int degen,
FLOAT cw_vec[2],
FLOAT ccw_vec[2],
FLOAT n_vec[2],
FLOAT d_vec[2],
FLOAT opt[2])
{
FLOAT d_cw, d_ccw, n_cw, n_ccw;
/* linear rational function case */
d_cw = dot2(cw_vec,d_vec);
d_ccw = dot2(ccw_vec,d_vec);
n_cw = dot2(cw_vec,n_vec);
n_ccw = dot2(ccw_vec,n_vec);
if(degen) {
/* if degenerate simply compare values */
if(n_cw/d_cw < n_ccw/d_ccw) {
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
} else {
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
/* check that the clock-wise and counter clockwise bounds are not near a poles */
} else if(not_zero(d_cw) && not_zero(d_ccw)) {
/* the valid region does not contain a poles */
if(d_cw*d_ccw > 0.0) {
/* find which end has the minimum value */
if(n_cw/d_cw < n_ccw/d_ccw) {
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
} else {
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
} else {
/* the valid region does contain a poles */
if(d_cw > 0.0) {
opt[0] = -d_vec[1];
opt[1] = d_vec[0];
} else {
opt[0] = d_vec[1];
opt[1] = -d_vec[0];
}
}
} else if(not_zero(d_cw)) {
/* the counter clockwise bound is near a pole */
if(n_ccw*d_cw > 0.0) {
/* counter clockwise bound is a positive pole */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
} else {
/* counter clockwise bound is a negative pole */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
} else if(not_zero(d_ccw)) {
/* the clockwise bound is near a pole */
if(n_cw*d_ccw > 2*EPS) {
/* clockwise bound is at a positive pole */
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
} else {
/* clockwise bound is at a negative pole */
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
}
} else {
/* both bounds are near poles */
if(cross2(d_vec,n_vec) > 0.0) {
opt[0] = cw_vec[0];
opt[1] = cw_vec[1];
} else {
opt[0] = ccw_vec[0];
opt[1] = ccw_vec[1];
}
}
}
scalar_t len_sq_2( vector2 v ) {
return dot2( v, v );
};
float norm2(float *v)
{
return sqrt(dot2(v, v));
}
/* Compute a circular arc fillet between lines L1 (p1 to p2)
and L2 (p3 to p4) with radius r.
The circle center is c.
The start angle is pa
The end angle is aa,
The points p1-p4 will be modified as necessary */
gboolean
fillet(Point *p1, Point *p2, Point *p3, Point *p4,
real r, Point *c, real *pa, real *aa)
{
real a1, b1, c1; /* Coefficients for L1 */
real a2, b2, c2; /* Coefficients for L2 */
real d, d1, d2;
real c1p, c2p;
real rr;
real start_angle, stop_angle;
Point mp,gv1,gv2;
gboolean righthand = FALSE;
line_coef(&a1,&b1,&c1,p1,p2);
line_coef(&a2,&b2,&c2,p3,p4);
if ( (a1*b2) == (a2*b1) ) /* Parallel or coincident lines */
return FALSE;
mp.x = (p3->x + p4->x) / 2.0; /* Find midpoint of p3 p4 */
mp.y = (p3->y + p4->y) / 2.0;
d1 = line_to_point(a1, b1, c1, &mp); /* Find distance p1 p2 to
midpoint p3 p4 */
if ( d1 == 0.0 ) return FALSE; /* p1p2 to p3 */
mp.x = (p1->x + p2->x) / 2.0; /* Find midpoint of p1 p2 */
mp.y = (p1->y + p2->y) / 2.0;
d2 = line_to_point(a2, b2, c2, &mp); /* Find distance p3 p4 to
midpoint p1 p2 */
if ( d2 == 0.0 ) return FALSE;
rr = r;
if ( d1 <= 0.0 ) rr = -rr;
c1p = c1-rr*sqrt((a1*a1)+(b1*b1)); /* Line parallell l1 at d */
rr = r;
if ( d2 <= 0.0 ) rr = -rr;
c2p = c2-rr*sqrt((a2*a2)+(b2*b2)); /* Line parallell l2 at d */
d = a1*b2-a2*b1;
c->x = ( c2p*b1-c1p*b2 ) / d; /* Intersect constructed lines */
c->y = ( c1p*a2-c2p*a1 ) / d; /* to find center of arc */
point_perp(c,a1,b1,c1,p2); /* Clip or extend lines as required */
point_perp(c,a2,b2,c2,p3);
/* need to negate the y coords to calculate angles correctly */
gv1.x = p2->x-c->x; gv1.y = -(p2->y-c->y);
gv2.x = p3->x-c->x; gv2.y = -(p3->y-c->y);
start_angle = atan2(gv1.y,gv1.x); /* Beginning angle for arc */
stop_angle = dot2(&gv1,&gv2);
if ( point_cross(&gv1,&gv2) < 0.0 ) {
stop_angle = -stop_angle; /* Angle subtended by arc */
/* also this means we need to swap our angles later on */
righthand = TRUE;
}
/* now calculate the actual angles in a form that the draw arc function
of the renderer can use */
start_angle = start_angle*180.0/G_PI;
stop_angle = start_angle + stop_angle*180.0/G_PI;
while (start_angle < 0.0) start_angle += 360.0;
while (stop_angle < 0.0) stop_angle += 360.0;
/* swap the start and stop if we had to negate the cross product */
if ( righthand ) {
real tmp = start_angle;
start_angle = stop_angle;
stop_angle = tmp;
}
*pa = start_angle;
*aa = stop_angle;
return TRUE;
}
inline void reflect2(vec2 * r, vec2 n) {
vec2 i = *r;
mult2( r, n, dot2( n, i ) * ((scalar)2) );
subFrom2( r, i );
}
inline void projectOnto2(vec2 a, vec2 * r) {
scalar l = length2(a) * length2(*r);
mult2( r, a, dot2(a, *r) );
invScale2( r, l );
}
// Projection functions
inline void projectedOnto2(vec2 * r, vec2 a, vec2 b) {
mult2( r, a, dot2(a, b) );
invScale2( r, length2(a) * length2(b) );
}
