1 /* 2 * Copyright (c) 2009-2010 jMonkeyEngine 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are 7 * met: 8 * 9 * * Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 12 * * Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * * Neither the name of 'jMonkeyEngine' nor the names of its contributors 17 * may be used to endorse or promote products derived from this software 18 * without specific prior written permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 22 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 23 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 24 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 25 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 26 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 27 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 28 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 29 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 30 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 */ 32 package com.jme3.bullet.joints; 33 34 import com.bulletphysics.dynamics.constraintsolver.Generic6DofConstraint; 35 import com.bulletphysics.linearmath.Transform; 36 import com.jme3.bullet.joints.motors.RotationalLimitMotor; 37 import com.jme3.bullet.joints.motors.TranslationalLimitMotor; 38 import com.jme3.bullet.objects.PhysicsRigidBody; 39 import com.jme3.bullet.util.Converter; 40 import com.jme3.export.InputCapsule; 41 import com.jme3.export.JmeExporter; 42 import com.jme3.export.JmeImporter; 43 import com.jme3.export.OutputCapsule; 44 import com.jme3.math.Matrix3f; 45 import com.jme3.math.Vector3f; 46 import java.io.IOException; 47 import java.util.Iterator; 48 import java.util.LinkedList; 49 50 /** 51 * <i>From bullet manual:</i><br> 52 * This generic constraint can emulate a variety of standard constraints, 53 * by configuring each of the 6 degrees of freedom (dof). 54 * The first 3 dof axis are linear axis, which represent translation of rigidbodies, 55 * and the latter 3 dof axis represent the angular motion. Each axis can be either locked, 56 * free or limited. On construction of a new btGeneric6DofConstraint, all axis are locked. 57 * Afterwards the axis can be reconfigured. Note that several combinations that 58 * include free and/or limited angular degrees of freedom are undefined. 59 * @author normenhansen 60 */ 61 public class SixDofJoint extends PhysicsJoint { 62 63 private boolean useLinearReferenceFrameA = true; 64 private LinkedList<RotationalLimitMotor> rotationalMotors = new LinkedList<RotationalLimitMotor>(); 65 private TranslationalLimitMotor translationalMotor; 66 private Vector3f angularUpperLimit = new Vector3f(Vector3f.POSITIVE_INFINITY); 67 private Vector3f angularLowerLimit = new Vector3f(Vector3f.NEGATIVE_INFINITY); 68 private Vector3f linearUpperLimit = new Vector3f(Vector3f.POSITIVE_INFINITY); 69 private Vector3f linearLowerLimit = new Vector3f(Vector3f.NEGATIVE_INFINITY); 70 SixDofJoint()71 public SixDofJoint() { 72 } 73 74 /** 75 * @param pivotA local translation of the joint connection point in node A 76 * @param pivotB local translation of the joint connection point in node B 77 */ SixDofJoint(PhysicsRigidBody nodeA, PhysicsRigidBody nodeB, Vector3f pivotA, Vector3f pivotB, Matrix3f rotA, Matrix3f rotB, boolean useLinearReferenceFrameA)78 public SixDofJoint(PhysicsRigidBody nodeA, PhysicsRigidBody nodeB, Vector3f pivotA, Vector3f pivotB, Matrix3f rotA, Matrix3f rotB, boolean useLinearReferenceFrameA) { 79 super(nodeA, nodeB, pivotA, pivotB); 80 this.useLinearReferenceFrameA = useLinearReferenceFrameA; 81 82 Transform transA = new Transform(Converter.convert(rotA)); 83 Converter.convert(pivotA, transA.origin); 84 Converter.convert(rotA, transA.basis); 85 86 Transform transB = new Transform(Converter.convert(rotB)); 87 Converter.convert(pivotB, transB.origin); 88 Converter.convert(rotB, transB.basis); 89 90 constraint = new Generic6DofConstraint(nodeA.getObjectId(), nodeB.getObjectId(), transA, transB, useLinearReferenceFrameA); 91 gatherMotors(); 92 } 93 94 /** 95 * @param pivotA local translation of the joint connection point in node A 96 * @param pivotB local translation of the joint connection point in node B 97 */ SixDofJoint(PhysicsRigidBody nodeA, PhysicsRigidBody nodeB, Vector3f pivotA, Vector3f pivotB, boolean useLinearReferenceFrameA)98 public SixDofJoint(PhysicsRigidBody nodeA, PhysicsRigidBody nodeB, Vector3f pivotA, Vector3f pivotB, boolean useLinearReferenceFrameA) { 99 super(nodeA, nodeB, pivotA, pivotB); 100 this.useLinearReferenceFrameA = useLinearReferenceFrameA; 101 102 Transform transA = new Transform(Converter.convert(new Matrix3f())); 103 Converter.convert(pivotA, transA.origin); 104 105 Transform transB = new Transform(Converter.convert(new Matrix3f())); 106 Converter.convert(pivotB, transB.origin); 107 108 constraint = new Generic6DofConstraint(nodeA.getObjectId(), nodeB.getObjectId(), transA, transB, useLinearReferenceFrameA); 109 gatherMotors(); 110 } 111 gatherMotors()112 private void gatherMotors() { 113 for (int i = 0; i < 3; i++) { 114 RotationalLimitMotor rmot = new RotationalLimitMotor(((Generic6DofConstraint) constraint).getRotationalLimitMotor(i)); 115 rotationalMotors.add(rmot); 116 } 117 translationalMotor = new TranslationalLimitMotor(((Generic6DofConstraint) constraint).getTranslationalLimitMotor()); 118 } 119 120 /** 121 * returns the TranslationalLimitMotor of this 6DofJoint which allows 122 * manipulating the translational axis 123 * @return the TranslationalLimitMotor 124 */ getTranslationalLimitMotor()125 public TranslationalLimitMotor getTranslationalLimitMotor() { 126 return translationalMotor; 127 } 128 129 /** 130 * returns one of the three RotationalLimitMotors of this 6DofJoint which 131 * allow manipulating the rotational axes 132 * @param index the index of the RotationalLimitMotor 133 * @return the RotationalLimitMotor at the given index 134 */ getRotationalLimitMotor(int index)135 public RotationalLimitMotor getRotationalLimitMotor(int index) { 136 return rotationalMotors.get(index); 137 } 138 setLinearUpperLimit(Vector3f vector)139 public void setLinearUpperLimit(Vector3f vector) { 140 linearUpperLimit.set(vector); 141 ((Generic6DofConstraint) constraint).setLinearUpperLimit(Converter.convert(vector)); 142 } 143 setLinearLowerLimit(Vector3f vector)144 public void setLinearLowerLimit(Vector3f vector) { 145 linearLowerLimit.set(vector); 146 ((Generic6DofConstraint) constraint).setLinearLowerLimit(Converter.convert(vector)); 147 } 148 setAngularUpperLimit(Vector3f vector)149 public void setAngularUpperLimit(Vector3f vector) { 150 angularUpperLimit.set(vector); 151 ((Generic6DofConstraint) constraint).setAngularUpperLimit(Converter.convert(vector)); 152 } 153 setAngularLowerLimit(Vector3f vector)154 public void setAngularLowerLimit(Vector3f vector) { 155 angularLowerLimit.set(vector); 156 ((Generic6DofConstraint) constraint).setAngularLowerLimit(Converter.convert(vector)); 157 } 158 159 @Override read(JmeImporter im)160 public void read(JmeImporter im) throws IOException { 161 super.read(im); 162 InputCapsule capsule = im.getCapsule(this); 163 164 Transform transA = new Transform(Converter.convert(new Matrix3f())); 165 Converter.convert(pivotA, transA.origin); 166 167 Transform transB = new Transform(Converter.convert(new Matrix3f())); 168 Converter.convert(pivotB, transB.origin); 169 constraint = new Generic6DofConstraint(nodeA.getObjectId(), nodeB.getObjectId(), transA, transB, useLinearReferenceFrameA); 170 gatherMotors(); 171 172 setAngularUpperLimit((Vector3f) capsule.readSavable("angularUpperLimit", new Vector3f(Vector3f.POSITIVE_INFINITY))); 173 setAngularLowerLimit((Vector3f) capsule.readSavable("angularLowerLimit", new Vector3f(Vector3f.NEGATIVE_INFINITY))); 174 setLinearUpperLimit((Vector3f) capsule.readSavable("linearUpperLimit", new Vector3f(Vector3f.POSITIVE_INFINITY))); 175 setLinearLowerLimit((Vector3f) capsule.readSavable("linearLowerLimit", new Vector3f(Vector3f.NEGATIVE_INFINITY))); 176 177 for (int i = 0; i < 3; i++) { 178 RotationalLimitMotor rotationalLimitMotor = getRotationalLimitMotor(i); 179 rotationalLimitMotor.setBounce(capsule.readFloat("rotMotor" + i + "_Bounce", 0.0f)); 180 rotationalLimitMotor.setDamping(capsule.readFloat("rotMotor" + i + "_Damping", 1.0f)); 181 rotationalLimitMotor.setERP(capsule.readFloat("rotMotor" + i + "_ERP", 0.5f)); 182 rotationalLimitMotor.setHiLimit(capsule.readFloat("rotMotor" + i + "_HiLimit", Float.POSITIVE_INFINITY)); 183 rotationalLimitMotor.setLimitSoftness(capsule.readFloat("rotMotor" + i + "_LimitSoftness", 0.5f)); 184 rotationalLimitMotor.setLoLimit(capsule.readFloat("rotMotor" + i + "_LoLimit", Float.NEGATIVE_INFINITY)); 185 rotationalLimitMotor.setMaxLimitForce(capsule.readFloat("rotMotor" + i + "_MaxLimitForce", 300.0f)); 186 rotationalLimitMotor.setMaxMotorForce(capsule.readFloat("rotMotor" + i + "_MaxMotorForce", 0.1f)); 187 rotationalLimitMotor.setTargetVelocity(capsule.readFloat("rotMotor" + i + "_TargetVelocity", 0)); 188 rotationalLimitMotor.setEnableMotor(capsule.readBoolean("rotMotor" + i + "_EnableMotor", false)); 189 } 190 getTranslationalLimitMotor().setAccumulatedImpulse((Vector3f) capsule.readSavable("transMotor_AccumulatedImpulse", Vector3f.ZERO)); 191 getTranslationalLimitMotor().setDamping(capsule.readFloat("transMotor_Damping", 1.0f)); 192 getTranslationalLimitMotor().setLimitSoftness(capsule.readFloat("transMotor_LimitSoftness", 0.7f)); 193 getTranslationalLimitMotor().setLowerLimit((Vector3f) capsule.readSavable("transMotor_LowerLimit", Vector3f.ZERO)); 194 getTranslationalLimitMotor().setRestitution(capsule.readFloat("transMotor_Restitution", 0.5f)); 195 getTranslationalLimitMotor().setUpperLimit((Vector3f) capsule.readSavable("transMotor_UpperLimit", Vector3f.ZERO)); 196 } 197 198 @Override write(JmeExporter ex)199 public void write(JmeExporter ex) throws IOException { 200 super.write(ex); 201 OutputCapsule capsule = ex.getCapsule(this); 202 capsule.write(angularUpperLimit, "angularUpperLimit", new Vector3f(Vector3f.POSITIVE_INFINITY)); 203 capsule.write(angularLowerLimit, "angularLowerLimit", new Vector3f(Vector3f.NEGATIVE_INFINITY)); 204 capsule.write(linearUpperLimit, "linearUpperLimit", new Vector3f(Vector3f.POSITIVE_INFINITY)); 205 capsule.write(linearLowerLimit, "linearLowerLimit", new Vector3f(Vector3f.NEGATIVE_INFINITY)); 206 int i = 0; 207 for (Iterator<RotationalLimitMotor> it = rotationalMotors.iterator(); it.hasNext();) { 208 RotationalLimitMotor rotationalLimitMotor = it.next(); 209 capsule.write(rotationalLimitMotor.getBounce(), "rotMotor" + i + "_Bounce", 0.0f); 210 capsule.write(rotationalLimitMotor.getDamping(), "rotMotor" + i + "_Damping", 1.0f); 211 capsule.write(rotationalLimitMotor.getERP(), "rotMotor" + i + "_ERP", 0.5f); 212 capsule.write(rotationalLimitMotor.getHiLimit(), "rotMotor" + i + "_HiLimit", Float.POSITIVE_INFINITY); 213 capsule.write(rotationalLimitMotor.getLimitSoftness(), "rotMotor" + i + "_LimitSoftness", 0.5f); 214 capsule.write(rotationalLimitMotor.getLoLimit(), "rotMotor" + i + "_LoLimit", Float.NEGATIVE_INFINITY); 215 capsule.write(rotationalLimitMotor.getMaxLimitForce(), "rotMotor" + i + "_MaxLimitForce", 300.0f); 216 capsule.write(rotationalLimitMotor.getMaxMotorForce(), "rotMotor" + i + "_MaxMotorForce", 0.1f); 217 capsule.write(rotationalLimitMotor.getTargetVelocity(), "rotMotor" + i + "_TargetVelocity", 0); 218 capsule.write(rotationalLimitMotor.isEnableMotor(), "rotMotor" + i + "_EnableMotor", false); 219 i++; 220 } 221 capsule.write(getTranslationalLimitMotor().getAccumulatedImpulse(), "transMotor_AccumulatedImpulse", Vector3f.ZERO); 222 capsule.write(getTranslationalLimitMotor().getDamping(), "transMotor_Damping", 1.0f); 223 capsule.write(getTranslationalLimitMotor().getLimitSoftness(), "transMotor_LimitSoftness", 0.7f); 224 capsule.write(getTranslationalLimitMotor().getLowerLimit(), "transMotor_LowerLimit", Vector3f.ZERO); 225 capsule.write(getTranslationalLimitMotor().getRestitution(), "transMotor_Restitution", 0.5f); 226 capsule.write(getTranslationalLimitMotor().getUpperLimit(), "transMotor_UpperLimit", Vector3f.ZERO); 227 } 228 } 229