org.ode4j.demo.ragdoll.DemoRagdoll Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of demo Show documentation
The newest version!
/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: [email protected]   Web: www.q12.org          *
 * Open Dynamics Engine 4J, Copyright (C) 2009-2014 Tilmann Zaeschke     *
 * All rights reserved.  Email: [email protected]   Web: www.ode4j.org        *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file ODE-LICENSE-BSD.TXT and ODE4J-LICENSE-BSD.TXT.         *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT, ODE-LICENSE-BSD.TXT and ODE4J-LICENSE-BSD.TXT for more   *
 * details.                                                              *
 *                                                                       *
 *************************************************************************/
package org.ode4j.demo.ragdoll;

import static org.ode4j.drawstuff.DrawStuff.dsDrawBox;
import static org.ode4j.drawstuff.DrawStuff.dsDrawCapsule;
import static org.ode4j.drawstuff.DrawStuff.dsSetColor;
import static org.ode4j.drawstuff.DrawStuff.dsSetViewpoint;
import static org.ode4j.drawstuff.DrawStuff.dsSimulationLoop;
import static org.ode4j.ode.OdeConstants.dContactBounce;
import static org.ode4j.ode.OdeConstants.dContactSoftCFM;
import static org.ode4j.ode.OdeHelper.areConnectedExcluding;

import org.ode4j.drawstuff.DrawStuff.dsFunctions;
import org.ode4j.math.DMatrix3;
import org.ode4j.math.DMatrix3C;
import org.ode4j.math.DQuaternion;
import org.ode4j.math.DVector3;
import org.ode4j.math.DVector3C;
import org.ode4j.ode.*;
import org.ode4j.ode.ragdoll.DRagdoll;

public class DemoRagdoll extends dsFunctions {

	private static final int  MAX_CONTACTS = 64;		// maximum number of contact points per body
	private DWorld world;
	private DSpace space;
	private DRagdoll ragdoll;
	private static final double[] xyz = {4.3966, -2.0614, 3.4300};
	private static final double[] hpr = {153.5, -14.5, 0};
	private DJointGroup contactgroup;
	private boolean show_contacts = false;	// show contact points?

	@Override
	public void start()
	{
		//dAllocateODEDataForThread(dAllocateMaskAll);
		dsSetViewpoint (xyz,hpr);
		System.out.println ("Press space to apply some force to the ragdoll");

		world = OdeHelper.createWorld();
		world.setGravity(0,0,-9.8);
		world.setDamping(1e-4, 1e-5);
		//    dWorldSetERP(world, 1);
		space = OdeHelper.createSimpleSpace();
		contactgroup = OdeHelper.createJointGroup ();
		OdeHelper.createPlane( space, 0, 0, 1, 0 );

		ragdoll = OdeHelper.createRagdoll(world, space, new DxDefaultHumanRagdollConfig());
		ragdoll.setAngularDamping(0.1);
		DQuaternion q = new DQuaternion(1, 0, 0, 0);
		DRotation.dQFromAxisAndAngle(q, new DVector3(1, 0, 0), -0.5 * Math.PI);
		for (DRagdoll.DRagdollBody bone : ragdoll.getBoneIter()) {
			DGeom g = OdeHelper.createCapsule(space, bone.getRadius(), bone.getLength());
			DBody body = bone.getBody();
			DQuaternion qq = new DQuaternion();
			OdeMath.dQMultiply1(qq, q, body.getQuaternion());
			body.setQuaternion(qq);
			DMatrix3 R = new DMatrix3();
			OdeMath.dRfromQ(R, q);
			DVector3 v = new DVector3();
			OdeMath.dMultiply0_133(v, body.getPosition(), R);
			body.setPosition(v.get0(), v.get1(), v.get2());
			g.setBody(body);
		}
		// initial camera position
		dsSetViewpoint (xyz,hpr);
	}

	@Override
	public void stop()
	{
		ragdoll.destroy();
		contactgroup.destroy();
		space.destroy();
		world.destroy();
	}


	private void drawGeom(DGeom g)
	{
		//int gclass = dGeomGetClass(g);
		if (g instanceof DCapsule) {
			DVector3C pos = g.getPosition();
			DMatrix3C rot = g.getRotation();
			DCapsule cap = (DCapsule) g; 
			dsDrawCapsule (pos, rot, cap.getLength(), cap.getRadius());
		}
	}

	private void nearCallback (Object data, DGeom o1, DGeom o2) {
		int i;
		// if (o1->body && o2->body) return;

		// exit without doing anything if the two bodies are connected by a joint
		DBody b1 = o1.getBody();
		DBody b2 = o2.getBody();
		if (b1!=null && b2!=null && areConnectedExcluding (b1,b2,DContactJoint.class)) {
			return;
		}

		DContactBuffer contacts = new DContactBuffer(MAX_CONTACTS);   // up to MAX_CONTACTS contacts per box-box
		for (i=0; i