private void manage(Entity e, float elapsedTime) {
Model model = e.get(Model.class);
if (!model.isCreated()) return;
PlanarStance stance = e.get(PlanarStance.class);
Spatial s = SpatialPool.models.get(e.getId());
// translation
s.setLocalTranslation(TranslateUtil.toVector3f(stance.getCoord().get3D(stance.getElevation())));
// rotation
Quaternion r = new Quaternion();
Point3D pu = stance.getUpVector();
Point3D pv = stance.getPlanarVector();
Vector3f u = TranslateUtil.toVector3f(pu).normalize();
Vector3f v = TranslateUtil.toVector3f(pv).normalize();
r.lookAt(v, u);
// we correct the pitch of the unit because the direction is always flatten
// this is only to follow the terrain relief
double angle = Math.acos(pu.getDotProduct(pv) / (pu.getNorm() * pv.getNorm()));
r =
r.mult(
new Quaternion()
.fromAngles(
(float) (-angle + AngleUtil.RIGHT + model.getPitchFix()),
(float) (model.getRollFix()),
(float) (model.getYawFix())));
s.setLocalRotation(r);
}
public void setSpatial(Spatial model) {
if (model == null) {
removeFromPhysicsSpace();
clearData();
return;
}
targetModel = model;
Node parent = model.getParent();
Vector3f initPosition = model.getLocalTranslation().clone();
Quaternion initRotation = model.getLocalRotation().clone();
initScale = model.getLocalScale().clone();
model.removeFromParent();
model.setLocalTranslation(Vector3f.ZERO);
model.setLocalRotation(Quaternion.IDENTITY);
model.setLocalScale(1);
// HACK ALERT change this
// I remove the skeletonControl and readd it to the spatial to make sure it's after the
// ragdollControl in the stack
// Find a proper way to order the controls.
SkeletonControl sc = model.getControl(SkeletonControl.class);
model.removeControl(sc);
model.addControl(sc);
// ----
removeFromPhysicsSpace();
clearData();
// put into bind pose and compute bone transforms in model space
// maybe dont reset to ragdoll out of animations?
scanSpatial(model);
if (parent != null) {
parent.attachChild(model);
}
model.setLocalTranslation(initPosition);
model.setLocalRotation(initRotation);
model.setLocalScale(initScale);
logger.log(Level.INFO, "Created physics ragdoll for skeleton {0}", skeleton);
}
/**
* For internal use only specific render for the ragdoll(if debugging)
*
* @param rm
* @param vp
*/
public void render(RenderManager rm, ViewPort vp) {
if (enabled && space != null && space.getDebugManager() != null) {
if (!debug) {
attachDebugShape(space.getDebugManager());
}
for (Iterator<PhysicsBoneLink> it = boneLinks.values().iterator(); it.hasNext(); ) {
PhysicsBoneLink physicsBoneLink = it.next();
Spatial debugShape = physicsBoneLink.rigidBody.debugShape();
if (debugShape != null) {
debugShape.setLocalTranslation(
physicsBoneLink.rigidBody.getMotionState().getWorldLocation());
debugShape.setLocalRotation(
physicsBoneLink.rigidBody.getMotionState().getWorldRotationQuat());
debugShape.updateGeometricState();
rm.renderScene(debugShape, vp);
}
}
}
}
/**
* <code>lookAt</code> is a convenience method for auto-setting the local rotation based on a
* position in world space and an up vector. It computes the rotation to transform the z-axis to
* point onto 'position' and the y-axis to 'up'. Unlike {@link
* Quaternion#lookAt(com.jme3.math.Vector3f, com.jme3.math.Vector3f) } this method takes a world
* position to look at and not a relative direction.
*
* <p>Note : 28/01/2013 this method has been fixed as it was not taking into account the parent
* rotation. This was resulting in improper rotation when the spatial had rotated parent nodes.
* This method is intended to work in world space, so no matter what parent graph the spatial has,
* it will look at the given position in world space.
*
* @param position where to look at in terms of world coordinates
* @param upVector a vector indicating the (local) up direction. (typically {0, 1, 0} in jME.)
*/
public void lookAt(Vector3f position, Vector3f upVector) {
Vector3f worldTranslation = getWorldTranslation();
TempVars vars = TempVars.get();
Vector3f compVecA = vars.vect4;
compVecA.set(position).subtractLocal(worldTranslation);
getLocalRotation().lookAt(compVecA, upVector);
if (getParent() != null) {
Quaternion rot = vars.quat1;
rot = rot.set(parent.getWorldRotation()).inverseLocal().multLocal(getLocalRotation());
rot.normalizeLocal();
setLocalRotation(rot);
}
vars.release();
setTransformRefresh();
}
public void setRotation(String objectID, float[] rotation) {
// get "visual" or "physical" spatial
// search in all sub-nodes of root node (scene node, trigger node, ...)
Spatial object = Util.findNode(sim.getRootNode(), objectID);
RigidBodyControl control = null;
try {
control = (RigidBodyControl) object.getControl(0);
} catch (IndexOutOfBoundsException e2) {
System.err.println("Could not manipulate physics of '" + objectID + "'!");
}
if (control != null) {
Quaternion rot = new Quaternion().fromAngles(degToRad(rotation));
control.setPhysicsRotation(rot);
} else {
Quaternion rot = new Quaternion().fromAngles(degToRad(rotation));
object.setLocalRotation(rot);
}
}
@Override
protected void onEntityEachTick(Entity e) {
Spatial s = Pool.models.get(e.getId());
if (s == null) return;
PlanarStance stance = e.get(PlanarStance.class);
// translation
s.setLocalTranslation(TranslateUtil.toVector3f(stance.coord.get3D(stance.elevation)));
// rotation
Quaternion r = new Quaternion();
Point3D pu = stance.upVector;
Point3D pv = Point3D.UNIT_X.getRotationAroundZ(stance.orientation.getValue());
Vector3f u = TranslateUtil.toVector3f(pu).normalize();
Vector3f v = TranslateUtil.toVector3f(pv).normalize();
r.lookAt(v, u);
double angle = Math.acos(pu.getDotProduct(pv) / (pu.getNorm() * pv.getNorm()));
r = r.mult(new Quaternion().fromAngles((float) (-angle), 0, 0));
s.setLocalRotation(r);
}
public void update(float tpf) {
if (!enabled) {
return;
}
TempVars vars = TempVars.get();
Quaternion tmpRot1 = vars.quat1;
Quaternion tmpRot2 = vars.quat2;
// if the ragdoll has the control of the skeleton, we update each bone with its position in
// physic world space.
if (mode == mode.Ragdoll && targetModel.getLocalTranslation().equals(modelPosition)) {
for (PhysicsBoneLink link : boneLinks.values()) {
Vector3f position = vars.vect1;
// retrieving bone position in physic world space
Vector3f p = link.rigidBody.getMotionState().getWorldLocation();
// transforming this position with inverse transforms of the model
targetModel.getWorldTransform().transformInverseVector(p, position);
// retrieving bone rotation in physic world space
Quaternion q = link.rigidBody.getMotionState().getWorldRotationQuat();
// multiplying this rotation by the initialWorld rotation of the bone,
// then transforming it with the inverse world rotation of the model
tmpRot1.set(q).multLocal(link.initalWorldRotation);
tmpRot2.set(targetModel.getWorldRotation()).inverseLocal().mult(tmpRot1, tmpRot1);
tmpRot1.normalizeLocal();
// if the bone is the root bone, we apply the physic's transform to the model, so its
// position and rotation are correctly updated
if (link.bone.getParent() == null) {
// offsetting the physic's position/rotation by the root bone inverse model space
// position/rotaion
modelPosition.set(p).subtractLocal(link.bone.getWorldBindPosition());
targetModel
.getParent()
.getWorldTransform()
.transformInverseVector(modelPosition, modelPosition);
modelRotation
.set(q)
.multLocal(tmpRot2.set(link.bone.getWorldBindRotation()).inverseLocal());
// applying transforms to the model
targetModel.setLocalTranslation(modelPosition);
targetModel.setLocalRotation(modelRotation);
// Applying computed transforms to the bone
link.bone.setUserTransformsWorld(position, tmpRot1);
} else {
// if boneList is empty, this means that every bone in the ragdoll has a collision shape,
// so we just update the bone position
if (boneList.isEmpty()) {
link.bone.setUserTransformsWorld(position, tmpRot1);
} else {
// boneList is not empty, this means some bones of the skeleton might not be associated
// with a collision shape.
// So we update them recusively
RagdollUtils.setTransform(link.bone, position, tmpRot1, false, boneList);
}
}
}
} else {
// the ragdoll does not have the controll, so the keyframed animation updates the physic
// position of the physic bonces
for (PhysicsBoneLink link : boneLinks.values()) {
Vector3f position = vars.vect1;
// if blended control this means, keyframed animation is updating the skeleton,
// but to allow smooth transition, we blend this transformation with the saved position of
// the ragdoll
if (blendedControl) {
Vector3f position2 = vars.vect2;
// initializing tmp vars with the start position/rotation of the ragdoll
position.set(link.startBlendingPos);
tmpRot1.set(link.startBlendingRot);
// interpolating between ragdoll position/rotation and keyframed position/rotation
tmpRot2.set(tmpRot1).nlerp(link.bone.getModelSpaceRotation(), blendStart / blendTime);
position2
.set(position)
.interpolate(link.bone.getModelSpacePosition(), blendStart / blendTime);
tmpRot1.set(tmpRot2);
position.set(position2);
// updating bones transforms
if (boneList.isEmpty()) {
// we ensure we have the control to update the bone
link.bone.setUserControl(true);
link.bone.setUserTransformsWorld(position, tmpRot1);
// we give control back to the key framed animation.
link.bone.setUserControl(false);
} else {
RagdollUtils.setTransform(link.bone, position, tmpRot1, true, boneList);
}
}
// setting skeleton transforms to the ragdoll
matchPhysicObjectToBone(link, position, tmpRot1);
modelPosition.set(targetModel.getLocalTranslation());
}
// time control for blending
if (blendedControl) {
blendStart += tpf;
if (blendStart > blendTime) {
blendedControl = false;
}
}
}
vars.release();
}
