159b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartapackage com.jme3.scene.plugins.blender.constraints; 259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 359b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.animation.Animation; 459b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.animation.Skeleton; 559b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.scene.plugins.blender.BlenderContext; 659b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.scene.plugins.blender.animations.CalculationBone; 759b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.scene.plugins.blender.animations.Ipo; 859b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.scene.plugins.blender.exceptions.BlenderFileException; 959b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.scene.plugins.blender.file.Structure; 1059b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport java.util.logging.Logger; 1159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 1259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta/** 1359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * This class represents 'Inverse kinematics' constraint type in blender. 1459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @author Marcin Roguski (Kaelthas) 1559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 1659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta/*package*/ class ConstraintInverseKinematics extends Constraint { 1759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta private static final Logger LOGGER = Logger.getLogger(ConstraintInverseKinematics.class.getName()); 1859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta private static final float IK_SOLVER_ERROR = 0.5f; 1959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 2059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 2159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * This constructor creates the constraint instance. 2259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 2359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param constraintStructure 2459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the constraint's structure (bConstraint clss in blender 2.49). 2559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param ownerOMA 2659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the old memory address of the constraint owner 2759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param influenceIpo 2859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the ipo curve of the influence factor 2959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param blenderContext 3059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the blender context 3159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @throws BlenderFileException 3259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * this exception is thrown when the blender file is somehow 3359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * corrupted 3459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 3559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public ConstraintInverseKinematics(Structure constraintStructure, 3659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta Long ownerOMA, Ipo influenceIpo, BlenderContext blenderContext) throws BlenderFileException { 3759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta super(constraintStructure, ownerOMA, influenceIpo, blenderContext); 3859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 3959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 4059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta @Override 4159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta protected void bakeConstraint() { 4259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// try { 4359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // IK solver is only attached to bones 4459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Bone ownerBone = (Bone) blenderContext.getLoadedFeature(ownerOMA, LoadedFeatureDataType.LOADED_FEATURE); 4559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// AnimData animData = blenderContext.getAnimData(ownerOMA); 4659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// if(animData == null) { 4759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta //TODO: to nie moxe byx null, utworzyx dane bez ruchu, w zalexnoxci czy target six rusza 4859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 4959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 5059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta //prepare a list of all parents of this bone 5159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// CalculationBone[] bones = this.getBonesToCalculate(skeleton, boneAnimation); 5259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 5359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // get the target point 5459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Object targetObject = this.getTarget(LoadedFeatureDataType.LOADED_FEATURE); 5559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Vector3f pt = null;// Point Target 5659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// if (targetObject instanceof Bone) { 5759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// pt = ((Bone) targetObject).getModelSpacePosition(); 5859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } else if (targetObject instanceof Spatial) { 5959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// pt = ((Spatial) targetObject).getWorldTranslation(); 6059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } else if (targetObject instanceof Skeleton) { 6159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Structure armatureNodeStructure = (Structure) this.getTarget(LoadedFeatureDataType.LOADED_STRUCTURE); 6259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// ObjectHelper objectHelper = blenderContext.getHelper(ObjectHelper.class); 6359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Transform transform = objectHelper.getTransformation(armatureNodeStructure, blenderContext); 6459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// pt = transform.getTranslation(); 6559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } else { 6659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// throw new IllegalStateException( 6759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// "Unknown target object type! Should be Node, Bone or Skeleton and there is: " 6859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// + targetObject.getClass().getName()); 6959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 7059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 7159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta //fetching the owner's bone track 7259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// BoneTrack ownerBoneTrack = null; 7359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// int boneIndex = skeleton.getBoneIndex(ownerBone); 7459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// for (int i = 0; i < boneAnimation.getTracks().length; ++i) { 7559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// if (boneAnimation.getTracks()[i].getTargetBoneIndex() == boneIndex) { 7659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// ownerBoneTrack = boneAnimation.getTracks()[i]; 7759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// break; 7859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 7959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 8059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// int ownerBoneFramesCount = ownerBoneTrack==null ? 0 : ownerBoneTrack.getTimes().length; 8159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// 8259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// // preparing data 8359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// int maxIterations = ((Number) data.getFieldValue("iterations")).intValue(); 8459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// CalculationBone[] bones = this.getBonesToCalculate(ownerBone, skeleton, boneAnimation); 8559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// for (int i = 0; i < bones.length; ++i) { 8659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// System.out.println(Arrays.toString(bones[i].track.getTranslations())); 8759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// System.out.println(Arrays.toString(bones[i].track.getRotations())); 8859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// System.out.println("==============================="); 8959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 9059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Quaternion rotation = new Quaternion(); 9159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// //all tracks should have the same amount of frames 9259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// int framesCount = bones[0].getBoneFramesCount(); 9359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// assert framesCount >=1; 9459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// for (int frame = 0; frame < framesCount; ++frame) { 9559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// float error = IK_SOLVER_ERROR; 9659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// int iteration = 0; 9759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// while (error >= IK_SOLVER_ERROR && iteration <= maxIterations) { 9859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// // rotating the bones 9959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// for (int i = 0; i < bones.length - 1; ++i) { 10059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Vector3f pe = bones[i].getEndPoint(); 10159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Vector3f pc = bones[i + 1].getWorldTranslation().clone(); 10259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// 10359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Vector3f peSUBpc = pe.subtract(pc).normalizeLocal(); 10459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Vector3f ptSUBpc = pt.subtract(pc).normalizeLocal(); 10559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// 10659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// float theta = FastMath.acos(peSUBpc.dot(ptSUBpc)); 10759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Vector3f direction = peSUBpc.cross(ptSUBpc).normalizeLocal(); 10859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// bones[i].rotate(rotation.fromAngleAxis(theta, direction), frame); 10959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 11059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// error = pt.subtract(bones[0].getEndPoint()).length(); 11159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// ++iteration; 11259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 11359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 11459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// 11559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// for (CalculationBone bone : bones) { 11659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// bone.applyCalculatedTracks(); 11759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 11859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// 11959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// System.out.println("&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&"); 12059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// for (int i = 0; i < bones.length; ++i) { 12159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// System.out.println(Arrays.toString(bones[i].track.getTranslations())); 12259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// System.out.println(Arrays.toString(bones[i].track.getRotations())); 12359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// System.out.println("==============================="); 12459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 12559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } catch(BlenderFileException e) { 12659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// LOGGER.severe(e.getLocalizedMessage()); 12759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 12859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 12959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 13059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 13159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * This method returns bones used for rotation calculations. 13259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param bone 13359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the bone to which the constraint is applied 13459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param skeleton 13559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the skeleton owning the bone and its ancestors 13659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param boneAnimation 13759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the bone animation data that stores the traces for the skeleton's bones 13859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return a list of bones to imitate the bone's movement during IK solving 13959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 14059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta private CalculationBone[] getBonesToCalculate(Skeleton skeleton, Animation boneAnimation) { 14159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// Bone ownerBone = (Bone) blenderContext.getLoadedFeature(ownerOMA, LoadedFeatureDataType.LOADED_FEATURE); 14259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// List<CalculationBone> bonesList = new ArrayList<CalculationBone>(); 14359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// do { 14459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// bonesList.add(new CalculationBone(ownerBone, 1)); 14559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// int boneIndex = skeleton.getBoneIndex(ownerBone); 14659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// for (int i = 0; i < boneAnimation.getTracks().length; ++i) { 14759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// if (((BoneTrack[])boneAnimation.getTracks())[i].getTargetBoneIndex() == boneIndex) { 14859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// bonesList.add(new CalculationBone(ownerBone, (BoneTrack)boneAnimation.getTracks()[i])); 14959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// break; 15059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 15159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 15259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// ownerBone = ownerBone.getParent(); 15359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } while (ownerBone != null); 15459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// //attaching children 15559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// CalculationBone[] result = bonesList.toArray(new CalculationBone[bonesList.size()]); 15659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// for (int i = result.length - 1; i > 0; --i) { 15759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// result[i].attachChild(result[i - 1]); 15859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// } 15959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta// return result; 16059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return null; 16159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 16259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta} 163