void main()
{
glfwInit();
// Create a window
window = glfwCreateWindow(800, 800, "Jarvis March", nullptr, nullptr);
glfwMakeContextCurrent(window);
glfwSwapInterval(0);
// Initializes most things needed before the main loop
init();
//Generate the point mesh
Vertex pointVertex;
pointVertex.x = pointVertex.y = pointVertex.z = pointVertex.r = 0.0f;
pointVertex.g = pointVertex.b = pointVertex.a = 1.0f;
//rope creation
point = new struct Mesh(1, &pointVertex, GL_POINTS);
//Scale the rope
point->scale = glm::scale(point->scale, glm::vec3(1.0f));
//Generate 25 rigidbodies
for(int i = 0; i < 25; i++)
{
float x = (static_cast <float> (rand()) / static_cast <float> (RAND_MAX)) - 0.5f;
float y = (static_cast <float> (rand()) / static_cast <float> (RAND_MAX)) - 0.5f;
bodies.push_back(RigidBody(glm::vec3(0.0f), glm::vec3(x, y, 0.0f)));
}
// Enter the main loop.
while (!glfwWindowShouldClose(window))
{
//Check time will update the programs clock and determine if & how many times the physics must be updated
checkTime();
// Call the render function.
renderScene();
// Swaps the back buffer to the front buffer
// Remember, you're rendering to the back buffer, then once rendering is complete, you're moving the back buffer to the front so it can be displayed.
glfwSwapBuffers(window);
// Checks to see if any events are pending and then processes them.
glfwPollEvents();
}
// After the program is over, cleanup your data!
glDeleteShader(vertex_shader);
glDeleteShader(fragment_shader);
glDeleteProgram(program);
// Note: If at any point you stop using a "program" or shaders, you should free the data up then and there.
delete point;
// Frees up GLFW memory
glfwTerminate();
}
kernel::ModelObjectsTemp RigidBodyUmbrella::do_get_inputs() const {
kernel::Model *m = get_model();
ModelObjectsTemp ret;
//reference rb
ret.push_back(m->get_particle(ref_));
kernel::ParticleIndexes pref(
RigidBody(m, ref_).get_member_indexes());
for (unsigned i=0; i<pref.size(); i++)
ret.push_back(m->get_particle(pref[i]));
//target rb
ret.push_back(m->get_particle(pi_));
kernel::ParticleIndexes iref(
RigidBody(m, pi_).get_member_indexes());
for (unsigned i=0; i<iref.size(); i++)
ret.push_back(m->get_particle(iref[i]));
return ret;
}
int main(int argc, const char *argv[]) try
{
std::vector<RigidBody> rbs;
for (auto const &b : bodysizes)
{
auto verts = genboxverts(b);
auto tris = calchull(verts, 8);
rbs.push_back(RigidBody({ Shape(verts, tris) }, float3(0, 0, 0)));
}
rbscalemass(&rbs[0], 5.0f); // make torso heavier than limb bones
rbs[0].position.z = 1.0f; // lift a meter off the ground.
DXWin mywin("Joint Drive - powered rag doll model", { 800,600 });
std::vector<Mesh> meshes;
for (auto &rb : rbs)
{
meshes.push_back(MeshSmoothish(rb.shapes[0].verts, rb.shapes[0].tris)); // 1 shape each is known
rb.damping = 0.8f; //rb.gravscale = 0;
}
for (auto &joint : joints)
{
rbs[joint.b0].ignore.push_back(&rbs[joint.b1]);
rbs[joint.b1].ignore.push_back(&rbs[joint.b0]);
rbs[joint.b1].position = rbs[joint.b0].pose() * joint.p0 - qrot(rbs[joint.b1].orientation,joint.p1);
}
WingMesh ground_wm = WingMeshBox({ -5, -5, -2.0f }, { 5, 5, -1.0f });
auto ground = MeshFlatShadeTex(ground_wm.verts, WingMeshTris(ground_wm));
ground.hack = { 0.25f, 0.75f, 0.25f, 1 };
Pose camera = { { 0, -8, 0 }, normalize(float4(0.9f, 0, 0, 1)) }; // where we view the rendered scene from.
float time = 0; // our global clock, used to generate the circular animation for upper limbs to follow
float torquelimit = 38.0f; // how much torque we let each joint apply each frame
while (mywin.WindowUp())
{
time += 0.06f;
std::vector<LimitAngular> angulars;
std::vector<LimitLinear> linears;
for (auto const &joint : joints)
{
Append(linears, ConstrainPositionNailed(&rbs[joint.b0], joint.p0, &rbs[joint.b1], joint.p1));
Append(angulars, ConstrainAngularDrive(&rbs[joint.b0], &rbs[joint.b1], (float4(0, joint.a*cos(time), joint.a*sin(time), sqrt(1.0f - joint.a*joint.a))), torquelimit));
}
PhysicsUpdate(Addresses<RigidBody>(rbs), linears, angulars, { &ground_wm.verts });
for (unsigned int i = 0; i < rbs.size(); i++)
meshes[i].pose = rbs[i].pose();
mywin.RenderScene(camera, Append(Addresses(meshes), std::vector<Mesh*>({ &ground })));
}
return 0;
}
catch (std::exception e)
{
MessageBoxA(GetActiveWindow(), e.what(), "FAIL", 0);
return -1;
}
void RiftEvaluator::evaluate(){
//Construct Frame from Rift Data
RBFrame *tempFrame = new RBFrame(mRifts.size(), 0, 0, 0, 0, true);
for(unsigned int i = 0; i < mRifts.size(); i++){
float pos[3]; float ori[4];
mRifts[i].first->getPose(pos, ori);
RigidBody *tempBody = new RigidBody(0, pos[0], pos[1], pos[2], ori[0], ori[1], ori[2], ori[3] );
tempBody->mID = mRifts[i].second;
(*tempFrame)[i]= tempBody;
}
delete mCurrentFrame;
mCurrentFrame = tempFrame;
//fish out history data
for(std::map<unsigned int, TimedFrame*>::iterator it = mRigidBodyHistories.begin(); it != mRigidBodyHistories.end() ; ++it){
for(unsigned int i = 0; i < mCurrentFrame->mNRigidBodies; i++){
if((*it).first == mCurrentFrame->mRbs[i]->mID){
TimedFrame* t = (*it).second;
if(t[mFrameBufferSize-1].mBody != nullptr){
delete t[mFrameBufferSize-1].mBody;
}
for(unsigned int j = mFrameBufferSize - 1; j > 0 ; j--){
t[j] = t[j-1];
}
t[0].mBody = new RigidBody(*mCurrentFrame->mRbs[i]);
LARGE_INTEGER tempStamp;
QueryPerformanceCounter(&tempStamp);
t[0].mTimestamp = tempStamp.QuadPart;
break;
}
}
}
//Let all RigidBodyEventListeners know
for(unsigned int i = 0; i < mRigidBodies.size(); i++){
for(unsigned int j = 0; j < mCurrentFrame->mNRigidBodies; j++){
if(mRigidBodies[i]->getRigidBodyID() == mCurrentFrame->mRbs[j]->mID){
if(mRigidBodies[i]->isCalibrating()){
mRigidBodies[i]->setReferenceOrientation(mCurrentFrame->mRbs[j]->mqX, mCurrentFrame->mRbs[j]->mqY, mCurrentFrame->mRbs[j]->mqZ, mCurrentFrame->mRbs[j]->mqW);
mRigidBodies[i]->setReferencePosition(mCurrentFrame->mRbs[j]->mX, mCurrentFrame->mRbs[j]->mY, mCurrentFrame->mRbs[j]->mZ);
mRigidBodies[i]->calibrate(false);
}
RigidBody rb = RigidBody();
RigidBodyEventListener* p = mRigidBodies[i];
RigidBody *q = mCurrentFrame->mRbs[j];
rb.mX = mCurrentFrame->mRbs[j]->mX;
rb.mY = mCurrentFrame->mRbs[j]->mY;
rb.mZ = mCurrentFrame->mRbs[j]->mZ;
mRigidBodies[i]->onChange(mCurrentFrame->mRbs[j]);
break;
}
}
}
}
Float RigidBodyAnglePairScore::evaluate_index(Model *m,
const ParticleIndexPair &pi,
DerivativeAccumulator *da) const {
IMP_UNUSED(da);
// check if derivatives are requested
IMP_USAGE_CHECK(!da, "Derivatives not implemented");
// check if rigid body
IMP_USAGE_CHECK(RigidBody::get_is_setup(m, pi[0]),
"Particle is not a rigid body");
IMP_USAGE_CHECK(RigidBody::get_is_setup(m, pi[1]),
"Particle is not a rigid body");
// principal axis of inertia is aligned to x axis when creating rigid body
algebra::Vector3D inertia=algebra::Vector3D(1.0,0.0,0.0);
algebra::Vector3D origin=algebra::Vector3D(0.0,0.0,0.0);
// get the two references frames
algebra::ReferenceFrame3D rf0 = RigidBody(m, pi[0]).get_reference_frame();
algebra::ReferenceFrame3D rf1 = RigidBody(m, pi[1]).get_reference_frame();
// rigid body 0
algebra::Vector3D i0 = rf0.get_global_coordinates(inertia);
algebra::Vector3D o0 = rf0.get_global_coordinates(origin);
// rigid body 1
algebra::Vector3D i1 = rf1.get_global_coordinates(inertia);
algebra::Vector3D o1 = rf1.get_global_coordinates(origin);
// now calculate the angle
Float sp=std::max(-1.0,std::min(1.0,(i1-o1).get_scalar_product(i0-o0)));
Float angle = acos(sp);
//std::cout << "ANGLE "<< angle <<std::endl;
Float score = f_->evaluate(angle);
return score;
}
IMPCORE_BEGIN_NAMESPACE
RigidBodyMover::RigidBodyMover(Model *m, ParticleIndex pi,
Float max_translation, Float max_angle)
: MonteCarloMover(m, m->get_particle(pi)->get_name() + " mover") {
IMP_USAGE_CHECK(RigidBody(m, pi).get_coordinates_are_optimized(),
"Rigid body passed to RigidBodyMover"
<< " must be set to be optimized. particle: "
<< m->get_particle_name(pi));
IMP_LOG_VERBOSE("start RigidBodyMover constructor");
max_translation_ = max_translation;
max_angle_ = max_angle;
pi_ = pi;
IMP_LOG_VERBOSE("finish mover construction" << std::endl);
}
/*--------------------------------------------------------------------------*/
extern int main
(
int argc,
const char
*argv[]
)
{ /* begin main */
float *ImageRasterArray, *OutputImage;
float *p;
double a11, a12, a21, a22;
double x0, y0, x1, y1;
double xOrigin, yOrigin;
double Angle, xShift, yShift;
long Width, Height;
long SplineDegree;
long x, y;
int Masking;
int Error;
/* access data samples */
Error = ReadByteImageRawData(&ImageRasterArray, &Width, &Height);
if (Error) {
printf("Failure to import image data\n");
return(1);
}
/* ask for transformation parameters */
RigidBody(&Angle, &xShift, &yShift, &xOrigin, &yOrigin, &SplineDegree, &Masking);
/* allocate output image */
OutputImage = (float *)malloc((size_t)(Width * Height * (long)sizeof(float)));
if (OutputImage == (float *)NULL) {
free(ImageRasterArray);
printf("Allocation of output image failed\n");
return(1);
}
/* convert between a representation based on image samples */
/* and a representation based on image B-spline coefficients */
Error = SamplesToCoefficients(ImageRasterArray, Width, Height, SplineDegree);
if (Error) {
free(OutputImage);
free(ImageRasterArray);
printf("Change of basis failed\n");
return(1);
}
/* prepare the geometry */
Angle *= PI / 180.0;
a11 = cos(Angle);
a12 = -sin(Angle);
a21 = sin(Angle);
a22 = cos(Angle);
x0 = a11 * (xShift + xOrigin) + a12 * (yShift + yOrigin);
y0 = a21 * (xShift + xOrigin) + a22 * (yShift + yOrigin);
xShift = xOrigin - x0;
yShift = yOrigin - y0;
/* visit all pixels of the output image and assign their value */
p = OutputImage;
for (y = 0L; y < Height; y++) {
x0 = a12 * (double)y + xShift;
y0 = a22 * (double)y + yShift;
for (x = 0L; x < Width; x++) {
x1 = x0 + a11 * (double)x;
y1 = y0 + a21 * (double)x;
if (Masking) {
if ((x1 <= -0.5) || (((double)Width - 0.5) <= x1)
|| (y1 <= -0.5) || (((double)Height - 0.5) <= y1)) {
*p++ = 0.0F;
}
else {
*p++ = (float)InterpolatedValue(ImageRasterArray, Width, Height,
x1, y1, SplineDegree);
}
}
else {
*p++ = (float)InterpolatedValue(ImageRasterArray, Width, Height,
x1, y1, SplineDegree);
}
}
}
/* save output */
Error = WriteByteImageRawData(OutputImage, Width, Height);
if (Error) {
free(OutputImage);
free(ImageRasterArray);
printf("Failure to export image data\n");
return(1);
}
free(OutputImage);
free(ImageRasterArray);
printf("Done\n");
return(0);
} /* end main */
int main(int argc, const char *argv[]) try
{
std::vector<Joint> joints;
std::vector<RigidBody> rbs;
std::map<std::string,unsigned int> rbindex;
auto xml = XMLParseFile("./default_hand.chr"); // replace string with whatever model you want to test. uses xml variation of John Ratcliff's easy mesh (ezm) file format.
auto const &skx = xml.child("model").child("skeleton");
for (auto const &b : skx.children)
{
rbindex[b.attribute("name")] = rbs.size();
auto verts = ArrayImport<float3>(b.child("verts").body);
auto tris = calchull(verts, verts.size());
float3 pos = StringTo<float3>(b.attribute("position"));
int parent = (b.hasAttribute("parent")) ? (int)rbindex[b.attribute("parent")] : -1;
rbs.push_back(RigidBody({ Shape(verts,tris) }, pos + ((parent>=0)?rbs[parent].position-rbs[parent].com:float3(0,0,0))));
if (parent>=0)
joints.push_back({ parent, (int)rbs.size() - 1, pos, float3(0,0,0), StringTo<float3>(b.child("jointlimitmin").body), StringTo<float3>(b.child("jointlimitmax").body) });
}
rbscalemass(&rbs[0], 3.0f);
rbscalemass(&rbs[1], 5.0f);
DXWin mywin("DX testing articulated rigged model", { 800,600 });
std::vector<Mesh> meshes;
for (auto &rb : rbs)
{
meshes.push_back(MeshSmoothish(rb.shapes[0].verts, rb.shapes[0].tris)); // 1 shape each is known
rb.damping = 0.8f;
//rb.gravscale = 0;
}
for (auto &joint : joints)
{
rbs[joint.rbi0].ignore.push_back(&rbs[joint.rbi1]);
rbs[joint.rbi1].ignore.push_back(&rbs[joint.rbi0]);
joint.p0 -= rbs[joint.rbi0].com;
joint.p1 -= rbs[joint.rbi1].com;
}
for (auto &ja : joints) for (auto &jb : joints) if (ja.rbi0 == jb.rbi0 && ja.rbi1 != jb.rbi1) // ignore siblings
{
rbs[ja.rbi1].ignore.push_back(&rbs[jb.rbi1]);
rbs[jb.rbi1].ignore.push_back(&rbs[ja.rbi1]);
}
for (auto &ja : joints) for (auto &jb : joints) if (ja.rbi1 == jb.rbi0 ) // ignore grandparents
{
rbs[ja.rbi0].ignore.push_back(&rbs[jb.rbi1]);
rbs[jb.rbi1].ignore.push_back(&rbs[ja.rbi0]);
}
std::vector<float3> groundpoints = { { -5.0f, -5.0f, -5.0f }, { 5.0f, -5.0f, -5.0f }, { 5.0f, 10.0f, -5.0f }, { -5.0f, 10.0f, -5.0f }, { -5.0f, -5.0f, -10.0f }, { 5.0f, -5.0f, -10.0f }, { 5.0f, 10.0f, -10.0f }, { -5.0f, 10.0f, -10.0f } };
Mesh ground = MeshSmoothish(groundpoints, { { 0, 1, 2 }, { 2, 3,0 } });
ground.hack = { 1, 1, 0 ,1};
WingMesh cube_wm = WingMeshCube(0.025f);
auto mesh_cube = MeshFlatShadeTex(cube_wm.verts, WingMeshTris(cube_wm));
mesh_cube.hack = { 0, 1, 0, 1 };
Pose camera = { { 0, -10, 0 }, normalize(float4(1, 0, 0, 1)) };
RigidBody *selected = NULL;
float3 spoint=camera * float3(0,0,-10);
float3 rbpoint;
struct Pin{ float3 w; RigidBody* rb; float3 p; };
std::vector<Pin> pins;
mywin.keyboardfunc = [&](int key, int, int)
{
if (key == 'g') for (auto &rb : rbs) rb.gravscale = 1.0f - rb.gravscale;
if (key == 'p' && selected)
Append<Pin>(pins, { spoint, selected, rbpoint });
};
while (mywin.WindowUp())
{
float3 ray = qrot(camera.orientation, normalize(mywin.MouseVector));
if (!selected)
{
for (auto &rb : rbs)
{
float3 v1 = camera.position + ray*100.0f;
if (auto h=ConvexHitCheck(Planes(rb.shapes[0].verts, rb.shapes[0].tris),rb.pose(),camera.position,v1))
{
v1 = h.impact;
selected = &rb;
spoint = h.impact;
rbpoint = rb.pose().inverse()*h.impact;
}
}
}
spoint = camera.position + ray * length(spoint - camera.position)*powf(1.025f, (float)mywin.mousewheel);
mesh_cube.pose.position = spoint;
if (!mywin.MouseState)
selected = NULL;
std::vector<LimitAngular> angulars;
std::vector<LimitLinear> linears;
for (auto const &joint : joints)
{
Append(linears, ConstrainPositionNailed(&rbs[joint.rbi0], joint.p0, &rbs[joint.rbi1], joint.p1));
Append(angulars, ConstrainAngularRange(&rbs[joint.rbi0], &rbs[joint.rbi1], { 0, 0, 0, 1 }, joint.jointlimitmin, joint.jointlimitmax));
}
if (selected)
Append(linears, ConstrainPositionNailed(NULL, spoint, selected, rbpoint));
for(auto &p:pins)
Append(linears, ConstrainPositionNailed(NULL, p.w,p.rb,p.p));
PhysicsUpdate(Addresses(rbs), linears, angulars, { &groundpoints });
for (unsigned int i = 0; i < rbs.size(); i++)
{
meshes[i].pose = rbs[i].pose();
}
mywin.RenderScene(camera, Append(Addresses(meshes),std::vector<Mesh*>({ &ground, &mesh_cube })));
}
}
catch (std::exception e)
{
MessageBoxA(GetActiveWindow(), e.what(), "FAIL", 0);
return -1;
}
