538 lines
15 KiB
C++
538 lines
15 KiB
C++
struct ragdollskel
|
|
{
|
|
struct vert
|
|
{
|
|
vec pos;
|
|
float radius, weight;
|
|
};
|
|
|
|
struct tri
|
|
{
|
|
int vert[3];
|
|
|
|
bool shareverts(const tri &t) const
|
|
{
|
|
loopi(3) loopj(3) if(vert[i] == t.vert[j]) return true;
|
|
return false;
|
|
}
|
|
};
|
|
|
|
struct distlimit
|
|
{
|
|
int vert[2];
|
|
float mindist, maxdist;
|
|
};
|
|
|
|
struct rotlimit
|
|
{
|
|
int tri[2];
|
|
float maxangle, maxtrace;
|
|
matrix3 middle;
|
|
};
|
|
|
|
struct rotfriction
|
|
{
|
|
int tri[2];
|
|
matrix3 middle;
|
|
};
|
|
|
|
struct joint
|
|
{
|
|
int bone, tri, vert[3];
|
|
float weight;
|
|
matrix4x3 orient;
|
|
};
|
|
|
|
struct reljoint
|
|
{
|
|
int bone, parent;
|
|
};
|
|
|
|
bool loaded, animjoints;
|
|
int eye;
|
|
vector<vert> verts;
|
|
vector<tri> tris;
|
|
vector<distlimit> distlimits;
|
|
vector<rotlimit> rotlimits;
|
|
vector<rotfriction> rotfrictions;
|
|
vector<joint> joints;
|
|
vector<reljoint> reljoints;
|
|
|
|
ragdollskel() : loaded(false), animjoints(false), eye(-1) {}
|
|
|
|
void setupjoints()
|
|
{
|
|
loopv(verts) verts[i].weight = 0;
|
|
loopv(joints)
|
|
{
|
|
joint &j = joints[i];
|
|
j.weight = 0;
|
|
vec pos(0, 0, 0);
|
|
loopk(3) if(j.vert[k]>=0)
|
|
{
|
|
pos.add(verts[j.vert[k]].pos);
|
|
j.weight++;
|
|
verts[j.vert[k]].weight++;
|
|
}
|
|
if(j.weight) j.weight = 1/j.weight;
|
|
pos.mul(j.weight);
|
|
|
|
tri &t = tris[j.tri];
|
|
matrix4x3 &m = j.orient;
|
|
const vec &v1 = verts[t.vert[0]].pos,
|
|
&v2 = verts[t.vert[1]].pos,
|
|
&v3 = verts[t.vert[2]].pos;
|
|
m.a = vec(v2).sub(v1).normalize();
|
|
m.c.cross(m.a, vec(v3).sub(v1)).normalize();
|
|
m.b.cross(m.c, m.a);
|
|
m.d = pos;
|
|
m.transpose();
|
|
}
|
|
loopv(verts) if(verts[i].weight) verts[i].weight = 1/verts[i].weight;
|
|
reljoints.shrink(0);
|
|
}
|
|
|
|
void setuprotfrictions()
|
|
{
|
|
rotfrictions.shrink(0);
|
|
loopv(tris) for(int j = i+1; j < tris.length(); j++) if(tris[i].shareverts(tris[j]))
|
|
{
|
|
rotfriction &r = rotfrictions.add();
|
|
r.tri[0] = i;
|
|
r.tri[1] = j;
|
|
}
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
setupjoints();
|
|
setuprotfrictions();
|
|
|
|
loaded = true;
|
|
}
|
|
|
|
void addreljoint(int bone, int parent)
|
|
{
|
|
reljoint &r = reljoints.add();
|
|
r.bone = bone;
|
|
r.parent = parent;
|
|
}
|
|
};
|
|
|
|
struct ragdolldata
|
|
{
|
|
struct vert
|
|
{
|
|
vec oldpos, pos, newpos, undo;
|
|
float weight;
|
|
bool collided, stuck;
|
|
|
|
vert() : oldpos(0, 0, 0), pos(0, 0, 0), newpos(0, 0, 0), undo(0, 0, 0), weight(0), collided(false), stuck(true) {}
|
|
};
|
|
|
|
ragdollskel *skel;
|
|
int millis, collidemillis, collisions, floating, lastmove, unsticks;
|
|
vec offset, center;
|
|
float radius, timestep, scale;
|
|
vert *verts;
|
|
matrix3 *tris;
|
|
matrix4x3 *animjoints;
|
|
dualquat *reljoints;
|
|
|
|
ragdolldata(ragdollskel *skel, float scale = 1)
|
|
: skel(skel),
|
|
millis(lastmillis),
|
|
collidemillis(0),
|
|
collisions(0),
|
|
floating(0),
|
|
lastmove(lastmillis),
|
|
unsticks(INT_MAX),
|
|
radius(0),
|
|
timestep(0),
|
|
scale(scale),
|
|
verts(new vert[skel->verts.length()]),
|
|
tris(new matrix3[skel->tris.length()]),
|
|
animjoints(!skel->animjoints || skel->joints.empty() ? nullptr : new matrix4x3[skel->joints.length()]),
|
|
reljoints(skel->reljoints.empty() ? nullptr : new dualquat[skel->reljoints.length()])
|
|
{
|
|
}
|
|
|
|
~ragdolldata()
|
|
{
|
|
delete[] verts;
|
|
delete[] tris;
|
|
if(animjoints) delete[] animjoints;
|
|
if(reljoints) delete[] reljoints;
|
|
}
|
|
|
|
void calcanimjoint(int i, const matrix4x3 &anim)
|
|
{
|
|
if(!animjoints) return;
|
|
ragdollskel::joint &j = skel->joints[i];
|
|
vec pos(0, 0, 0);
|
|
loopk(3) if(j.vert[k]>=0) pos.add(verts[j.vert[k]].pos);
|
|
pos.mul(j.weight);
|
|
|
|
ragdollskel::tri &t = skel->tris[j.tri];
|
|
matrix4x3 m;
|
|
const vec &v1 = verts[t.vert[0]].pos,
|
|
&v2 = verts[t.vert[1]].pos,
|
|
&v3 = verts[t.vert[2]].pos;
|
|
m.a = vec(v2).sub(v1).normalize();
|
|
m.c.cross(m.a, vec(v3).sub(v1)).normalize();
|
|
m.b.cross(m.c, m.a);
|
|
m.d = pos;
|
|
animjoints[i].transposemul(m, anim);
|
|
}
|
|
|
|
void calctris()
|
|
{
|
|
loopv(skel->tris)
|
|
{
|
|
ragdollskel::tri &t = skel->tris[i];
|
|
matrix3 &m = tris[i];
|
|
const vec &v1 = verts[t.vert[0]].pos,
|
|
&v2 = verts[t.vert[1]].pos,
|
|
&v3 = verts[t.vert[2]].pos;
|
|
m.a = vec(v2).sub(v1).normalize();
|
|
m.c.cross(m.a, vec(v3).sub(v1)).normalize();
|
|
m.b.cross(m.c, m.a);
|
|
}
|
|
}
|
|
|
|
void calcboundsphere()
|
|
{
|
|
center = vec(0, 0, 0);
|
|
loopv(skel->verts) center.add(verts[i].pos);
|
|
center.div(skel->verts.length());
|
|
radius = 0;
|
|
loopv(skel->verts) radius = max(radius, verts[i].pos.dist(center));
|
|
}
|
|
|
|
void init(dynent *d)
|
|
{
|
|
extern int ragdolltimestepmin;
|
|
float ts = ragdolltimestepmin/1000.0f;
|
|
loopv(skel->verts) (verts[i].oldpos = verts[i].pos).sub(vec(d->vel).add(d->falling).mul(ts));
|
|
timestep = ts;
|
|
|
|
calctris();
|
|
calcboundsphere();
|
|
offset = d->o;
|
|
offset.sub(skel->eye >= 0 ? verts[skel->eye].pos : center);
|
|
offset.z += (d->eyeheight + d->aboveeye)/2;
|
|
}
|
|
|
|
void move(dynent *pl, float ts);
|
|
void constrain();
|
|
void constraindist();
|
|
void applyrotlimit(ragdollskel::tri &t1, ragdollskel::tri &t2, float angle, const vec &axis);
|
|
void constrainrot();
|
|
void calcrotfriction();
|
|
void applyrotfriction(float ts);
|
|
void tryunstick(float speed);
|
|
|
|
static inline bool collidevert(const vec &pos, const vec &dir, float radius)
|
|
{
|
|
static struct vertent : physent
|
|
{
|
|
vertent()
|
|
{
|
|
type = ENT_BOUNCE;
|
|
radius = xradius = yradius = eyeheight = aboveeye = 1;
|
|
}
|
|
} v;
|
|
v.o = pos;
|
|
if(v.radius != radius) v.radius = v.xradius = v.yradius = v.eyeheight = v.aboveeye = radius;
|
|
return collide(&v, dir, 0, false);
|
|
}
|
|
};
|
|
|
|
/*
|
|
seed particle position = avg(modelview * base2anim * spherepos)
|
|
mapped transform = invert(curtri) * origtrig
|
|
parented transform = parent{invert(curtri) * origtrig} * (invert(parent{base2anim}) * base2anim)
|
|
*/
|
|
|
|
void ragdolldata::constraindist()
|
|
{
|
|
float invscale = 1.0f/scale;
|
|
loopv(skel->distlimits)
|
|
{
|
|
ragdollskel::distlimit &d = skel->distlimits[i];
|
|
vert &v1 = verts[d.vert[0]], &v2 = verts[d.vert[1]];
|
|
vec dir = vec(v2.pos).sub(v1.pos);
|
|
float dist = dir.magnitude()*invscale, cdist;
|
|
if(dist < d.mindist) cdist = d.mindist;
|
|
else if(dist > d.maxdist) cdist = d.maxdist;
|
|
else continue;
|
|
if(dist > 1e-4f) dir.mul(cdist*0.5f/dist);
|
|
else dir = vec(0, 0, cdist*0.5f/invscale);
|
|
vec center = vec(v1.pos).add(v2.pos).mul(0.5f);
|
|
v1.newpos.add(vec(center).sub(dir));
|
|
v1.weight++;
|
|
v2.newpos.add(vec(center).add(dir));
|
|
v2.weight++;
|
|
}
|
|
}
|
|
|
|
inline void ragdolldata::applyrotlimit(ragdollskel::tri &t1, ragdollskel::tri &t2, float angle, const vec &axis)
|
|
{
|
|
vert &v1a = verts[t1.vert[0]], &v1b = verts[t1.vert[1]], &v1c = verts[t1.vert[2]],
|
|
&v2a = verts[t2.vert[0]], &v2b = verts[t2.vert[1]], &v2c = verts[t2.vert[2]];
|
|
vec m1 = vec(v1a.pos).add(v1b.pos).add(v1c.pos).div(3),
|
|
m2 = vec(v2a.pos).add(v2b.pos).add(v2c.pos).div(3),
|
|
q1a, q1b, q1c, q2a, q2b, q2c;
|
|
float w1 = q1a.cross(axis, vec(v1a.pos).sub(m1)).magnitude() +
|
|
q1b.cross(axis, vec(v1b.pos).sub(m1)).magnitude() +
|
|
q1c.cross(axis, vec(v1c.pos).sub(m1)).magnitude(),
|
|
w2 = q2a.cross(axis, vec(v2a.pos).sub(m2)).magnitude() +
|
|
q2b.cross(axis, vec(v2b.pos).sub(m2)).magnitude() +
|
|
q2c.cross(axis, vec(v2c.pos).sub(m2)).magnitude();
|
|
angle /= w1 + w2 + 1e-9f;
|
|
float a1 = angle*w2, a2 = -angle*w1,
|
|
s1 = sinf(a1), s2 = sinf(a2);
|
|
vec c1 = vec(axis).mul(1 - cosf(a1)), c2 = vec(axis).mul(1 - cosf(a2));
|
|
v1a.newpos.add(vec().cross(c1, q1a).madd(q1a, s1).add(v1a.pos));
|
|
v1a.weight++;
|
|
v1b.newpos.add(vec().cross(c1, q1b).madd(q1b, s1).add(v1b.pos));
|
|
v1b.weight++;
|
|
v1c.newpos.add(vec().cross(c1, q1c).madd(q1c, s1).add(v1c.pos));
|
|
v1c.weight++;
|
|
v2a.newpos.add(vec().cross(c2, q2a).madd(q2a, s2).add(v2a.pos));
|
|
v2a.weight++;
|
|
v2b.newpos.add(vec().cross(c2, q2b).madd(q2b, s2).add(v2b.pos));
|
|
v2b.weight++;
|
|
v2c.newpos.add(vec().cross(c2, q2c).madd(q2c, s2).add(v2c.pos));
|
|
v2c.weight++;
|
|
}
|
|
|
|
void ragdolldata::constrainrot()
|
|
{
|
|
calctris();
|
|
loopv(skel->rotlimits)
|
|
{
|
|
ragdollskel::rotlimit &r = skel->rotlimits[i];
|
|
matrix3 rot;
|
|
rot.mul(tris[r.tri[0]], r.middle);
|
|
rot.multranspose(tris[r.tri[1]]);
|
|
|
|
vec axis;
|
|
float angle, tr = rot.trace();
|
|
if(tr >= r.maxtrace || !rot.calcangleaxis(tr, angle, axis)) continue;
|
|
|
|
angle = r.maxangle - angle + 1e-3f;
|
|
applyrotlimit(skel->tris[r.tri[0]], skel->tris[r.tri[1]], angle, axis);
|
|
}
|
|
}
|
|
|
|
VAR(ragdolltimestepmin, 1, 5, 50);
|
|
VAR(ragdolltimestepmax, 1, 10, 50);
|
|
FVAR(ragdollrotfric, 0, 0.85f, 1);
|
|
FVAR(ragdollrotfricstop, 0, 0.1f, 1);
|
|
|
|
void ragdolldata::calcrotfriction()
|
|
{
|
|
loopv(skel->rotfrictions)
|
|
{
|
|
ragdollskel::rotfriction &r = skel->rotfrictions[i];
|
|
r.middle.transposemul(tris[r.tri[0]], tris[r.tri[1]]);
|
|
}
|
|
}
|
|
|
|
void ragdolldata::applyrotfriction(float ts)
|
|
{
|
|
calctris();
|
|
float stopangle = 2*M_PI*ts*ragdollrotfricstop, rotfric = 1.0f - pow(ragdollrotfric, ts*1000.0f/ragdolltimestepmin);
|
|
loopv(skel->rotfrictions)
|
|
{
|
|
ragdollskel::rotfriction &r = skel->rotfrictions[i];
|
|
matrix3 rot;
|
|
rot.mul(tris[r.tri[0]], r.middle);
|
|
rot.multranspose(tris[r.tri[1]]);
|
|
|
|
vec axis;
|
|
float angle;
|
|
if(!rot.calcangleaxis(angle, axis)) continue;
|
|
|
|
angle *= -(fabs(angle) >= stopangle ? rotfric : 1.0f);
|
|
applyrotlimit(skel->tris[r.tri[0]], skel->tris[r.tri[1]], angle, axis);
|
|
}
|
|
loopv(skel->verts)
|
|
{
|
|
vert &v = verts[i];
|
|
if(!v.weight) continue;
|
|
v.pos = v.newpos.div(v.weight);
|
|
v.newpos = vec(0, 0, 0);
|
|
v.weight = 0;
|
|
}
|
|
}
|
|
|
|
void ragdolldata::tryunstick(float speed)
|
|
{
|
|
vec unstuck(0, 0, 0);
|
|
int stuck = 0;
|
|
loopv(skel->verts)
|
|
{
|
|
vert &v = verts[i];
|
|
if(v.stuck)
|
|
{
|
|
if(collidevert(v.pos, vec(0, 0, 0), skel->verts[i].radius)) { stuck++; continue; }
|
|
v.stuck = false;
|
|
}
|
|
unstuck.add(v.pos);
|
|
}
|
|
unsticks = 0;
|
|
if(!stuck || stuck >= skel->verts.length()) return;
|
|
unstuck.div(skel->verts.length() - stuck);
|
|
loopv(skel->verts)
|
|
{
|
|
vert &v = verts[i];
|
|
if(v.stuck)
|
|
{
|
|
v.pos.add(vec(unstuck).sub(v.pos).rescale(speed));
|
|
unsticks++;
|
|
}
|
|
}
|
|
}
|
|
|
|
VAR(ragdollconstrain, 1, 7, 100);
|
|
|
|
void ragdolldata::constrain()
|
|
{
|
|
loopi(ragdollconstrain)
|
|
{
|
|
constraindist();
|
|
loopvj(skel->verts)
|
|
{
|
|
vert &v = verts[j];
|
|
v.undo = v.pos;
|
|
if(v.weight)
|
|
{
|
|
v.pos = v.newpos.div(v.weight);
|
|
v.newpos = vec(0, 0, 0);
|
|
v.weight = 0;
|
|
}
|
|
}
|
|
|
|
constrainrot();
|
|
loopvj(skel->verts)
|
|
{
|
|
vert &v = verts[j];
|
|
if(v.weight)
|
|
{
|
|
v.pos = v.newpos.div(v.weight);
|
|
v.newpos = vec(0, 0, 0);
|
|
v.weight = 0;
|
|
}
|
|
if(v.pos != v.undo && collidevert(v.pos, vec(v.pos).sub(v.undo), skel->verts[j].radius))
|
|
{
|
|
vec dir = vec(v.pos).sub(v.oldpos);
|
|
float facing = dir.dot(collidewall);
|
|
if(facing < 0) v.oldpos = vec(v.undo).sub(dir.msub(collidewall, 2*facing));
|
|
v.pos = v.undo;
|
|
v.collided = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
FVAR(ragdollbodyfric, 0, 0.95f, 1);
|
|
FVAR(ragdollbodyfricscale, 0, 2, 10);
|
|
FVAR(ragdollwaterfric, 0, 0.85f, 1);
|
|
FVAR(ragdollgroundfric, 0, 0.8f, 1);
|
|
FVAR(ragdollairfric, 0, 0.996f, 1);
|
|
FVAR(ragdollunstick, 0, 10, 1e3f);
|
|
VAR(ragdollexpireoffset, 0, 2500, 30000);
|
|
VAR(ragdollwaterexpireoffset, 0, 4000, 30000);
|
|
|
|
void ragdolldata::move(dynent *pl, float ts)
|
|
{
|
|
extern const float GRAVITY;
|
|
if(collidemillis && lastmillis > collidemillis) return;
|
|
|
|
int material = lookupmaterial(vec(center.x, center.y, center.z + radius/2));
|
|
bool water = isliquid(material&MATF_VOLUME);
|
|
if(!pl->inwater && water) game::physicstrigger(pl, true, 0, -1, material&MATF_VOLUME);
|
|
else if(pl->inwater && !water)
|
|
{
|
|
material = lookupmaterial(center);
|
|
water = isliquid(material&MATF_VOLUME);
|
|
if(!water) game::physicstrigger(pl, true, 0, 1, pl->inwater);
|
|
}
|
|
pl->inwater = water ? material&MATF_VOLUME : MAT_AIR;
|
|
|
|
calcrotfriction();
|
|
float tsfric = timestep ? ts/timestep : 1,
|
|
airfric = ragdollairfric + min((ragdollbodyfricscale*collisions)/skel->verts.length(), 1.0f)*(ragdollbodyfric - ragdollairfric);
|
|
collisions = 0;
|
|
loopv(skel->verts)
|
|
{
|
|
vert &v = verts[i];
|
|
vec dpos = vec(v.pos).sub(v.oldpos);
|
|
dpos.z -= GRAVITY*ts*ts;
|
|
if(water) dpos.z += 0.25f*sinf(detrnd(size_t(this)+i, 360)*RAD + lastmillis/10000.0f*M_PI)*ts;
|
|
dpos.mul(pow((water ? ragdollwaterfric : 1.0f) * (v.collided ? ragdollgroundfric : airfric), ts*1000.0f/ragdolltimestepmin)*tsfric);
|
|
v.oldpos = v.pos;
|
|
v.pos.add(dpos);
|
|
}
|
|
applyrotfriction(ts);
|
|
loopv(skel->verts)
|
|
{
|
|
vert &v = verts[i];
|
|
if(v.pos.z < 0) { v.pos.z = 0; v.oldpos = v.pos; collisions++; }
|
|
vec dir = vec(v.pos).sub(v.oldpos);
|
|
v.collided = collidevert(v.pos, dir, skel->verts[i].radius);
|
|
if(v.collided)
|
|
{
|
|
v.pos = v.oldpos;
|
|
v.oldpos.sub(dir.reflect(collidewall));
|
|
collisions++;
|
|
}
|
|
}
|
|
|
|
if(unsticks && ragdollunstick) tryunstick(ts*ragdollunstick);
|
|
|
|
timestep = ts;
|
|
if(collisions)
|
|
{
|
|
floating = 0;
|
|
if(!collidemillis) collidemillis = lastmillis + (water ? ragdollwaterexpireoffset : ragdollexpireoffset);
|
|
}
|
|
else if(++floating > 1 && lastmillis < collidemillis) collidemillis = 0;
|
|
|
|
constrain();
|
|
calctris();
|
|
calcboundsphere();
|
|
}
|
|
|
|
FVAR(ragdolleyesmooth, 0, 0.5f, 1);
|
|
VAR(ragdolleyesmoothmillis, 1, 250, 10000);
|
|
|
|
void moveragdoll(dynent *d)
|
|
{
|
|
if(!curtime || !d->ragdoll) return;
|
|
|
|
if(!d->ragdoll->collidemillis || lastmillis < d->ragdoll->collidemillis)
|
|
{
|
|
int lastmove = d->ragdoll->lastmove;
|
|
while(d->ragdoll->lastmove + (lastmove == d->ragdoll->lastmove ? ragdolltimestepmin : ragdolltimestepmax) <= lastmillis)
|
|
{
|
|
int timestep = min(ragdolltimestepmax, lastmillis - d->ragdoll->lastmove);
|
|
d->ragdoll->move(d, timestep/1000.0f);
|
|
d->ragdoll->lastmove += timestep;
|
|
}
|
|
}
|
|
|
|
vec eye = d->ragdoll->skel->eye >= 0 ? d->ragdoll->verts[d->ragdoll->skel->eye].pos : d->ragdoll->center;
|
|
eye.add(d->ragdoll->offset);
|
|
float k = pow(ragdolleyesmooth, float(curtime)/ragdolleyesmoothmillis);
|
|
d->o.lerp(eye, 1-k);
|
|
}
|
|
|
|
void cleanragdoll(dynent *d)
|
|
{
|
|
DELETEP(d->ragdoll);
|
|
}
|
|
|