#include <cmath>
#include "Camera.h"
#include "Skeleton.h"
#include "misc.h"
extern float multiplier;
extern unsigned int gSourceID[37];
extern int visions;
extern float rad2deg;
extern Camera camera;
extern float soundscalefactor;
extern XYZ vel;
extern XYZ midp;
extern XYZ newpoint1,newpoint2;
extern float oldlength;
extern float relaxlength;
extern float friction;
extern int numrepeats;
extern float groundlevel;
extern float offset;
extern XYZ impact;
extern XYZ overpoint;
extern XYZ underpoint;
extern int whichtri;
extern XYZ normalrotated;
extern bool groundish;
void Joint::DoConstraint()
{
if(hasparent){
//Find midpoint
midp=(position+parent->position)/2;
// Find vector from midpoint to second vector
vel = normalize(parent->position - midp);
//Apply velocity change
velocity+=((midp-vel*length/2)-position);
parent->velocity+=((midp+vel*length/2)-parent->position);
//Move child point to within certain distance of parent point
if(!locked)position=midp-vel*length/2;
if(!parent->locked)parent->position=midp+vel*length/2;
}
}
void Muscle::DoConstraint(int broken)
{
oldlength=length;
relaxlength=findDistance(parent1->position,parent2->position);
if(type==boneconnect)strength=1;
if(type==constraint)strength=0;
if(strength<0)strength=0;
if(strength>1)strength=1;
length-=(length-relaxlength)*(1-strength)*multiplier*multiplier*10000;
length-=(length-targetlength)*(strength)*multiplier*multiplier*10000;
if(strength==0)length=relaxlength;
if((relaxlength-length>0&&relaxlength-oldlength<0)||(relaxlength-length<0&&relaxlength-oldlength>0))length=relaxlength;
if(length<minlength)length=minlength;
if(length>maxlength&&!broken)length=maxlength;
//Find midpoint
midp=(parent1->position+parent2->position)/2;
// Find vector from midpoint to second vector
vel = normalize(parent2->position - midp);
//Apply velocity change
newpoint1=midp-vel*length/2;
newpoint2=midp+vel*length/2;
parent1->velocity+=(newpoint1-parent1->position);
parent2->velocity+=(newpoint2-parent2->position);
//Move child point to within certain distance of parent point
if(!parent1->locked)parent1->position=newpoint1;
if(!parent2->locked)parent2->position=newpoint2;
}
void Skeleton::DoConstraints()
{
numrepeats = 3;
for(int i = 0; i < numrepeats; ++i)
for (auto& joint : joints)
joint.DoConstraint();
}
void Skeleton::DoConstraints(Model *collide, XYZ *move, float rotation)
{
friction = 20;
move->y += 0.35;
groundlevel = 0;
numrepeats = 2;
for (int j = 0; j < numrepeats; j++) {
for (int i = 0; i < max_joints; i++) {
if (!joints[i].existing && i != lefthand && i != righthand)
continue;
// Length constraints
joints[i].DoConstraint();
// Ground constraint
auto& pos = joints[i].position;
overpoint = pos;
overpoint.y += 10;
underpoint = pos;
underpoint.y -= offset = 0;
whichtri = collide->LineCheck2(overpoint, underpoint,
&impact, *move, rotation);
if (whichtri == -1
|| collide->normals[whichtri].y <= 0.8)
whichtri = collide->LineCheck2(
joints[i].realoldposition, pos,
&impact, *move, rotation);
if (pos.y <= groundlevel + offset || whichtri != -1) {
if (whichtri == -1
|| collide->normals[whichtri].y > 0.8) {
if (whichtri == -1) {
pos.y = groundlevel + offset;
} else {
pos = impact;
pos.y += offset;
}
joints[i].velocity.y *= -0.3;
joints[i].velocity.x *= 0.3;
joints[i].velocity.z *= 0.3;
}
offset = true;
if (whichtri != -1
&& collide->normals[whichtri].y <= 0.8) {
normalrotated = DoRotation(collide->normals[whichtri], 0, rotation, 0);
pos = impact + normalrotated * offset;
reflect(&joints[i].velocity, normalrotated);
joints[i].velocity *= 0.3;
}
if (broken > 1)
continue;
XYZ avgvelocity {0};
for (int k = 0; k < max_joints; k++)
avgvelocity += joints[k].velocity;
avgvelocity /= max_joints;
int landsound = -1;
switch (joints[i].label) {
case head:
landsound = headlandsound;
break;
case abdomen:
landsound = bodylandsound;
break;
}
ALint playing = AL_PLAYING;
if (landsound >= 0)
alGetSourcei(gSourceID[landsound],
AL_SOURCE_STATE, &playing);
if (playing != AL_PLAYING
&& (sqrlen(joints[i].velocity) > 2
|| sqrlen(avgvelocity) > 2)) {
auto soundpos = pos - camera.position;
playSound(gSourceID[landsound],
soundpos.x, soundpos.y, soundpos.z);
}
}
}
for (int i = 0; i < num_muscles; i++)
//Length constraints
muscles[i].DoConstraint(broken);
}
for (int i = 0; i < max_joints; i++) {
joints[i].realoldposition = joints[i].position;
//Add velocity
if (joints[i].existing || i == lefthand || i == righthand)
joints[i].position += joints[i].velocity * multiplier;
}
}
void Skeleton::DoGravity()
{
for (auto& joint : joints)
joint.velocity.y += gravity * multiplier;
}
void Skeleton::Draw(int muscleview)
{
float jointcolor[4];
if(muscleview!=2){
jointcolor[0]=0;
jointcolor[1]=0;
jointcolor[2]=.5;
jointcolor[3]=1;
}
if(muscleview==2){
jointcolor[0]=0;
jointcolor[1]=0;
jointcolor[2]=0;
jointcolor[3]=.5;
}
if(muscleview!=2){
glPointSize(3);
glBegin(GL_POINTS);
for(int i=0; i<max_joints; i++){
if(i!=selected)glColor4f(0,0,.5,1);
if(i==selected)glColor4f(1,1,0,1);
if(joints[i].locked&&i!=selected)glColor4f(1,0,0,1);
glVertex3f(joints[i].position.x,joints[i].position.y,joints[i].position.z);
}
glEnd();
}
//Set old position to current position
if(muscleview==2)
for(int i=0; i<max_joints; i++){
joints[i].oldposition=joints[i].position;
}
glDepthMask(1);
}
void Skeleton::DeleteJoint(int whichjoint)
{
if (whichjoint < max_joints && whichjoint >= 0) {
for(int i=0;i<num_muscles;i++){
while(muscles[i].parent1==&joints[whichjoint]&&i<num_muscles)DeleteMuscle(i);
while(muscles[i].parent2==&joints[whichjoint]&&i<num_muscles)DeleteMuscle(i);
}
for (auto& joint : joints)
if (joint.parent == &joints[whichjoint])
joint.hasparent = 0;
joints[whichjoint].hasparent = 0;
}
}
void Skeleton::DeleteMuscle(int whichmuscle)
{
if(whichmuscle<num_muscles){
muscles[whichmuscle].minlength=muscles[num_muscles-1].minlength;
muscles[whichmuscle].maxlength=muscles[num_muscles-1].maxlength;
muscles[whichmuscle].strength=muscles[num_muscles-1].strength;
muscles[whichmuscle].parent1=muscles[num_muscles-1].parent1;
muscles[whichmuscle].parent2=muscles[num_muscles-1].parent2;
muscles[whichmuscle].length=muscles[num_muscles-1].length;
muscles[whichmuscle].visible=muscles[num_muscles-1].visible;
muscles[whichmuscle].type=muscles[num_muscles-1].type;
muscles[whichmuscle].targetlength=muscles[num_muscles-1].targetlength;
num_muscles--;
}
}
void Skeleton::FindRotationJoint(int which)
{
XYZ temppoint1,temppoint2,tempforward;
float distance;
temppoint1=joints[which].position;
temppoint2=joints[which].parent->position;
distance=findDistance(temppoint1,temppoint2);
joints[which].rotate2=asin((temppoint1.y-temppoint2.y)/distance)*rad2deg;
temppoint1.y=0;
temppoint2.y=0;
joints[which].rotate1=acos((temppoint1.z-temppoint2.z)/findDistance(temppoint1,temppoint2))*rad2deg;
if(temppoint1.x>temppoint2.x)joints[which].rotate1=360-joints[which].rotate1;
if(joints[which].label==head)tempforward=specialforward[0];
else if(joints[which].label==rightshoulder||joints[which].label==rightelbow||joints[which].label==rightwrist||joints[which].label==righthand)tempforward=specialforward[1];
else if(joints[which].label==leftshoulder||joints[which].label==leftelbow||joints[which].label==leftwrist||joints[which].label==lefthand)tempforward=specialforward[2];
else if(joints[which].label==righthip||joints[which].label==rightknee||joints[which].label==rightankle)tempforward=specialforward[3];
else if(joints[which].label==lefthip||joints[which].label==leftknee||joints[which].label==leftankle)tempforward=specialforward[4];
else if(!joints[which].lower)tempforward=forward;
else if(joints[which].lower)tempforward=lowforward;
tempforward=DoRotation(tempforward,0,joints[which].rotate1-90,0);
tempforward=DoRotation(tempforward,0,0,joints[which].rotate2-90);
tempforward.y=0;
tempforward = normalize(tempforward);
joints[which].rotate3=acos(0-tempforward.z)*rad2deg;
if(0>tempforward.x)joints[which].rotate3=360-joints[which].rotate3;
}
void Skeleton::FindRotationMuscle(int which)
{
XYZ temppoint1,temppoint2,tempforward;
float distance;
temppoint1=muscles[which].parent1->position;
temppoint2=muscles[which].parent2->position;
distance=findDistance(temppoint1,temppoint2);
muscles[which].rotate2=asin((temppoint1.y-temppoint2.y)/distance)*rad2deg;
temppoint1.y=0;
temppoint2.y=0;
muscles[which].rotate1=acos((temppoint1.z-temppoint2.z)/findDistance(temppoint1,temppoint2));
muscles[which].rotate1*=360/6.28;
if(temppoint1.x>temppoint2.x)muscles[which].rotate1=360-muscles[which].rotate1;
if(muscles[which].parent1->label==head)tempforward=specialforward[0];
else if(muscles[which].parent1->label==rightshoulder||muscles[which].parent1->label==rightelbow||muscles[which].parent1->label==rightwrist)tempforward=specialforward[1];
else if(muscles[which].parent1->label==leftshoulder||muscles[which].parent1->label==leftelbow||muscles[which].parent1->label==leftwrist)tempforward=specialforward[2];
else if(muscles[which].parent1->label==righthip||muscles[which].parent1->label==rightknee||muscles[which].parent1->label==rightankle)tempforward=specialforward[3];
else if(muscles[which].parent1->label==lefthip||muscles[which].parent1->label==leftknee||muscles[which].parent1->label==leftankle)tempforward=specialforward[4];
else if(!muscles[which].parent1->lower)tempforward=forward;
else if(muscles[which].parent1->lower)tempforward=lowforward;
tempforward=DoRotation(tempforward,0,muscles[which].rotate1-90,0);
tempforward=DoRotation(tempforward,0,0,muscles[which].rotate2-90);
tempforward.y=0;
tempforward = normalize(tempforward);
muscles[which].rotate3=acos(0-tempforward.z)*rad2deg;
for (auto& joint : joints)
if (&joint == muscles[which].parent1) {
joint.rotate1=muscles[which].rotate1;
joint.rotate2=muscles[which].rotate2;
joint.rotate3=muscles[which].rotate3;
}
if(0>tempforward.x)muscles[which].rotate3=360-muscles[which].rotate3;
}
extern Skeleton testskeleton;
void Animation::load(const char* name)
{
auto data = loadAnimation(name);
numframes = data.len;
for (size_t i = 0; i < numframes; ++i) {
for (size_t j = 0; j < max_joints; ++j) {
position[j][i].x = data.ptr[i].joints[j].x;
position[j][i].y = data.ptr[i].joints[j].y;
position[j][i].z = data.ptr[i].joints[j].z;
}
speed[i] = data.ptr[i].speed;
}
free(data.ptr);
for(int j=0;j<numframes;j++){
for (int i = 0; i < max_joints; ++i)
testskeleton.joints[i].position = position[i][j];
//Find forward vectors
testskeleton.forward = normalize(crossProduct(testskeleton.joints[testskeleton.forwardjoints[1]].position
- testskeleton.joints[testskeleton.forwardjoints[0]].position,
testskeleton.joints[testskeleton.forwardjoints[2]].position
- testskeleton.joints[testskeleton.forwardjoints[0]].position));
testskeleton.lowforward = normalize(crossProduct(testskeleton.joints[testskeleton.lowforwardjoints[1]].position
- testskeleton.joints[testskeleton.lowforwardjoints[0]].position,
testskeleton.joints[testskeleton.lowforwardjoints[2]].position
- testskeleton.joints[testskeleton.lowforwardjoints[0]].position));
//Special forwards
testskeleton.specialforward[0]=testskeleton.forward;
testskeleton.specialforward[1]=testskeleton.joints[rightshoulder].position+testskeleton.joints[rightwrist].position;
testskeleton.specialforward[1]=testskeleton.joints[rightelbow].position-testskeleton.specialforward[1]/2;
testskeleton.specialforward[1]+=testskeleton.forward*.2;
testskeleton.specialforward[1] = normalize(testskeleton.specialforward[1]);
testskeleton.specialforward[2]=testskeleton.joints[leftshoulder].position+testskeleton.joints[leftwrist].position;
testskeleton.specialforward[2]=testskeleton.joints[leftelbow].position-testskeleton.specialforward[2]/2;
testskeleton.specialforward[2]+=testskeleton.forward*.2;
testskeleton.specialforward[2] = normalize(testskeleton.specialforward[2]);
testskeleton.specialforward[3]=testskeleton.joints[righthip].position+testskeleton.joints[rightankle].position;
testskeleton.specialforward[3]=testskeleton.specialforward[3]/2-testskeleton.joints[rightknee].position;
testskeleton.specialforward[3]+=testskeleton.lowforward*.2;
testskeleton.specialforward[3] = normalize(testskeleton.specialforward[3]);
testskeleton.specialforward[4]=testskeleton.joints[lefthip].position+testskeleton.joints[leftankle].position;
testskeleton.specialforward[4]=testskeleton.specialforward[4]/2-testskeleton.joints[leftknee].position;
testskeleton.specialforward[4]+=testskeleton.lowforward*.2;
testskeleton.specialforward[4] = normalize(testskeleton.specialforward[4]);
//Find joint rotations
for (int i = 0; i < max_joints; ++i)
if (testskeleton.joints[i].hasparent
&& testskeleton.joints[i].visible) {
testskeleton.FindRotationJoint(i);
rotate1[i][j]=testskeleton.joints[i].rotate1;
rotate2[i][j]=testskeleton.joints[i].rotate2;
rotate3[i][j]=testskeleton.joints[i].rotate3;
if(j!=0&&rotate3[i][j]>rotate3[i][j-1]+180)rotate3[i][j]-=360;
if(j!=0&&rotate3[i][j]<rotate3[i][j-1]-180)rotate3[i][j]+=360;
if(j!=0&&rotate2[i][j]>rotate2[i][j-1]+180)rotate2[i][j]-=360;
if(j!=0&&rotate2[i][j]<rotate2[i][j-1]-180)rotate2[i][j]+=360;
if(j!=0&&rotate1[i][j]>rotate1[i][j-1]+180)rotate1[i][j]-=360;
if(j!=0&&rotate1[i][j]<rotate1[i][j-1]-180)rotate1[i][j]+=360;
}
for (int i = 0; i < max_muscles; ++i)
if (testskeleton.muscles[i].visible) {
testskeleton.FindRotationMuscle(i);
mrotate1[i][j]=testskeleton.muscles[i].rotate1;
mrotate2[i][j]=testskeleton.muscles[i].rotate2;
mrotate3[i][j]=testskeleton.muscles[i].rotate3;
if(j!=0&&mrotate3[i][j]>mrotate3[i][j-1]+180)mrotate3[i][j]-=360;
if(j!=0&&mrotate3[i][j]<mrotate3[i][j-1]-180)mrotate3[i][j]+=360;
if(j!=0&&mrotate2[i][j]>mrotate2[i][j-1]+180)mrotate2[i][j]-=360;
if(j!=0&&mrotate2[i][j]<mrotate2[i][j-1]-180)mrotate2[i][j]+=360;
if(j!=0&&mrotate1[i][j]>mrotate1[i][j-1]+180)mrotate1[i][j]-=360;
if(j!=0&&mrotate1[i][j]<mrotate1[i][j-1]-180)mrotate1[i][j]+=360;
}
}
for(int k=0;k<2;k++)
for(int j=0;j<numframes;j++){
for (int i = 0; i < max_muscles; ++i)
if(testskeleton.muscles[i].visible) {
if(j!=0&&mrotate3[i][j]>mrotate3[i][j-1]+180)mrotate3[i][j]-=360;
if(j!=0&&mrotate3[i][j]<mrotate3[i][j-1]-180)mrotate3[i][j]+=360;
if(j!=0&&mrotate2[i][j]>mrotate2[i][j-1]+180)mrotate2[i][j]-=360;
if(j!=0&&mrotate2[i][j]<mrotate2[i][j-1]-180)mrotate2[i][j]+=360;
if(j!=0&&mrotate1[i][j]>mrotate1[i][j-1]+180)mrotate1[i][j]-=360;
if(j!=0&&mrotate1[i][j]<mrotate1[i][j-1]-180)mrotate1[i][j]+=360;
if(j==0&&mrotate3[i][j]>mrotate3[i][numframes-1]+180)mrotate3[i][j]-=360;
if(j==0&&mrotate3[i][j]<mrotate3[i][numframes-1]-180)mrotate3[i][j]+=360;
if(j==0&&mrotate2[i][j]>mrotate2[i][numframes-1]+180)mrotate2[i][j]-=360;
if(j==0&&mrotate2[i][j]<mrotate2[i][numframes-1]-180)mrotate2[i][j]+=360;
if(j==0&&mrotate1[i][j]>mrotate1[i][numframes-1]+180)mrotate1[i][j]-=360;
if(j==0&&mrotate1[i][j]<mrotate1[i][numframes-1]-180)mrotate1[i][j]+=360;
}
for (int i = 0; i < max_joints; ++i)
if(testskeleton.joints[i].hasparent&&testskeleton.joints[i].visible) {
if(j!=0&&rotate3[i][j]>rotate3[i][j-1]+180)rotate3[i][j]-=360;
if(j!=0&&rotate3[i][j]<rotate3[i][j-1]-180)rotate3[i][j]+=360;
if(j!=0&&rotate2[i][j]>rotate2[i][j-1]+180)rotate2[i][j]-=360;
if(j!=0&&rotate2[i][j]<rotate2[i][j-1]-180)rotate2[i][j]+=360;
if(j!=0&&rotate1[i][j]>rotate1[i][j-1]+180)rotate1[i][j]-=360;
if(j!=0&&rotate1[i][j]<rotate1[i][j-1]-180)rotate1[i][j]+=360;
if(j==0&&rotate3[i][j]>rotate3[i][numframes-1]+180)rotate3[i][j]-=360;
if(j==0&&rotate3[i][j]<rotate3[i][numframes-1]-180)rotate3[i][j]+=360;
if(j==0&&rotate2[i][j]>rotate2[i][numframes-1]+180)rotate2[i][j]-=360;
if(j==0&&rotate2[i][j]<rotate2[i][numframes-1]-180)rotate2[i][j]+=360;
if(j==0&&rotate1[i][j]>rotate1[i][numframes-1]+180)rotate1[i][j]-=360;
if(j==0&&rotate1[i][j]<rotate1[i][numframes-1]-180)rotate1[i][j]+=360;
}
}
}
void Skeleton::reload()
{
broken = 0;
JointData joints_data[max_joints];
loadJoints(joints_data);
for (int i = 0; i < max_joints; ++i) {
joints[i].label = i;
joints[i].position.x = joints_data[i].x;
joints[i].position.y = joints_data[i].y;
joints[i].position.z = joints_data[i].z;
joints[i].length = joints_data[i].length;
joints[i].modelnum = joints_data[i].model;
joints[i].visible = joints_data[i].visible;
joints[i].lower = joints_data[i].lower;
if (joints_data[i].parent < 0) {
joints[i].hasparent = false;
joints[i].parent = NULL;
} else {
joints[i].hasparent = true;
joints[i].parent = joints + joints_data[i].parent;
}
joints[i].existing = true;
joints[i].locked = false;
joints[i].velocity = {};
joints[i].oldposition = joints[i].position;
}
forwardjoints[0] = leftshoulder;
forwardjoints[1] = abdomen;
forwardjoints[2] = rightshoulder;
lowforwardjoints[0] = righthip;
lowforwardjoints[1] = abdomen;
lowforwardjoints[2] = lefthip;
num_muscles = 29;
MuscleData muscles_data[num_muscles];
loadMuscles(muscles_data);
for (int i = 0; i < num_muscles; ++i) {
muscles[i].length = muscles_data[i].length;
muscles[i].targetlength = muscles_data[i].initlen;
muscles[i].minlength = muscles_data[i].minlen;
muscles[i].maxlength = muscles_data[i].maxlen;
muscles[i].type = muscles_data[i].flag;
muscles[i].visible = muscles_data[i].visible;
muscles[i].parent1 = joints + muscles_data[i].parent1;
muscles[i].parent2 = joints + muscles_data[i].parent2;
muscles[i].strength = !muscles[i].type;
}
}
