CCD IK Solver
implement Cyclic-Coordinate Descent algorithm.
HOW TO:
press key to move target position
x-axis - [A] [D]
z-axis - [W] [S]
y-axis - [Up] [Down]
2014-01-11 18:32:48
/**
* Copyright civet ( http://wonderfl.net/user/civet )
* MIT License ( http://www.opensource.org/licenses/mit-license.php )
* Downloaded from: http://wonderfl.net/c/y4t9
*/
//2014-01-11 18:32:48
package
{
import away3d.containers.ObjectContainer3D;
import away3d.containers.View3D;
import away3d.controllers.HoverController;
import away3d.core.math.Quaternion;
import away3d.entities.Mesh;
import away3d.lights.DirectionalLight;
import away3d.lights.PointLight;
import away3d.lights.shadowmaps.NearDirectionalShadowMapper;
import away3d.materials.ColorMaterial;
import away3d.materials.lightpickers.StaticLightPicker;
import away3d.materials.methods.FilteredShadowMapMethod;
import away3d.materials.methods.NearShadowMapMethod;
import away3d.primitives.ConeGeometry;
import away3d.primitives.PlaneGeometry;
import away3d.primitives.SphereGeometry;
import away3d.tools.helpers.MeshHelper;
import flash.display.Bitmap;
import flash.display.BitmapData;
import flash.display.Sprite;
import flash.display.StageAlign;
import flash.display.StageScaleMode;
import flash.events.Event;
import flash.events.KeyboardEvent;
import flash.events.MouseEvent;
import flash.geom.Matrix3D;
import flash.geom.Vector3D;
import flash.ui.Keyboard;
public class TestCCD extends Sprite
{
//engine
private var _view:View3D;
private var _cameraController:HoverController;
private var source:BitmapData = new BitmapData(465, 465, true, 0x00);
public function TestCCD()
{
Wonderfl.disable_capture();
addChild(new Bitmap(source));
//http://wonderfl.net/c/wia8
initAway3D();
initObjects();
}
public function initAway3D():void
{
stage.scaleMode = StageScaleMode.NO_SCALE;
stage.align = StageAlign.TOP_LEFT;
stage.frameRate = 60;
//setup the view
_view = new View3D();
_view.antiAlias = 2;//2x
addChild(_view);
//setup the scene
var ground:Mesh = new Mesh(new PlaneGeometry(400, 400)/*, new ColorMaterial()*/);
_view.scene.addChild(ground);
//setup the camera
_cameraController = new HoverController(_view.camera, ground, 180, 15, 400);
//ground and shadow
var light:DirectionalLight = new DirectionalLight(0, -1, 1);
light.color = 0xffffff;
light.shadowMapper = new NearDirectionalShadowMapper(.2);
_view.scene.addChild(light);
var shadowMethod:NearShadowMapMethod = new NearShadowMapMethod(new FilteredShadowMapMethod(light));
shadowMethod.epsilon = 1;
shadowMethod.alpha = 0.5;
var groundMat:ColorMaterial = new ColorMaterial(0xffffff);
groundMat.lightPicker = new StaticLightPicker([light]);
groundMat.shadowMethod = shadowMethod;
groundMat.gloss = 4;
ground.castsShadows = false;
ground.material = groundMat;
//frame loop
stage.addEventListener(Event.ENTER_FRAME, onEnterFrame);
//autosize
stage.addEventListener(Event.RESIZE, onResize);
onResize();
//camera controls
stage.addEventListener(MouseEvent.MOUSE_DOWN, onMouseDown);
stage.addEventListener(MouseEvent.MOUSE_UP, onMouseUp);
stage.addEventListener(MouseEvent.MOUSE_WHEEL, onMouseWheel);
//keyboard
stage.addEventListener(KeyboardEvent.KEY_DOWN, onKeyDown);
stage.addEventListener(KeyboardEvent.KEY_UP, onKeyUp);
}
//---------
public const NUM_JOINTS:int = 4;
private var _joints:Vector.<JointObject>;
private var _chain:Vector.<int>;//0(effector), 1, 2...
private var _targetObject:ObjectContainer3D;
private var _iterations:int = 8;
private var _minDistance:int = 1;
public function initObjects():void
{
//init joints
_joints = new Vector.<JointObject>();
var joint:JointObject;
var i:int, num:int = NUM_JOINTS;
for(i = 0; i < num; ++i) {
joint = new JointObject( new Vector3D(0, 50 * i, 0) )
var display:ObjectContainer3D = createJointDisplay();
var boneDisplay:ObjectContainer3D = (i < num-1) ? createBoneDisplay() : createBoneDisplay(0x00ff00, 6, 3);
display.addChild( boneDisplay );
_view.scene.addChild( display );
joint.setDisplay( display );
joint.updateDisplay();
_joints.push(joint);
}
//init chain
_chain = new Vector.<int>();
for(i = num-1; i >= 0; --i) {
_chain.push( i );
joint = _joints[i];
joint.parent = i - 1;//Root: -1
if(joint.parent >= 0) {
joint.relativePosition = joint.position.subtract( _joints[ joint.parent ].position );
}
else {
joint.relativePosition = new Vector3D(0, 0, 0);
}
}
//init target
_targetObject = createJointDisplay(0x00ff00);
_targetObject.position = new Vector3D(80, 50, 0);
_view.scene.addChild(_targetObject);
calculateIK( _targetObject.position );
}
public function calculateIK(targetPosition:Vector3D):void
{
var itr:int = _iterations;
while(itr--) {
var num:int = (itr == 0 ? 2 : _chain.length);
for(var i:int = 1; i < num; ++i) {
//current joint (as root) and effector
var joint:JointObject = _joints[ _chain[i] ];
var effector:JointObject = _joints[ _chain[0] ];
//var targetPosition:Vector3D = ;
var jointPositon:Vector3D = joint.position;
var effectorPositon:Vector3D = effector.position;
var v0:Vector3D = targetPosition.subtract(jointPositon);
v0.normalize();
var v1:Vector3D = effectorPositon.subtract(jointPositon);
v1.normalize();
var angle:Number = Math.acos( v1.dotProduct(v0) );
var axis:Vector3D = v1.crossProduct(v0);
axis.normalize();
var rotation:Quaternion = new Quaternion();
rotation.fromAxisAngle(axis, angle);
//apply rotation
joint.rotation.multiply(rotation, joint.rotation);
joint.updateDisplay();
//update children
for(var j:int = i-1; j >= 0; --j)
{
var childJoint:JointObject = _joints[ _chain[j] ];
//if(childJoint.parent == -1) continue;
var parentJoint:JointObject = _joints[ childJoint.parent ];
var relativePos:Vector3D = parentJoint.rotation.rotatePoint( childJoint.relativePosition );
childJoint.position = parentJoint.position.add( relativePos );
childJoint.rotation.multiply(rotation, childJoint.rotation);
childJoint.updateDisplay();
}
//if nearest
if(effector.position.subtract( targetPosition ).length < _minDistance ) return;
}
}
}
//------ Utils ------
private function createJointDisplay(color:uint=0xff0000, radius:int=3):ObjectContainer3D
{
var light:PointLight = new PointLight();
light.x = 0;
light.y = 200;
light.z = -200;
light.color = 0xffffff;
var mat:ColorMaterial = new ColorMaterial(color);
mat.lightPicker = new StaticLightPicker([light]);
mat.gloss = 2;
var joint:ObjectContainer3D = new ObjectContainer3D();
var mesh:Mesh = new Mesh(new SphereGeometry(radius, 8, 6), mat);
joint.addChild(mesh);
return joint;
}
private function createBoneDisplay(color:uint=0xff0000, length:int=50, radius:int=6):ObjectContainer3D
{
var light:PointLight = new PointLight();
light.x = 0;
light.y = 200;
light.z = -200;
light.color = 0xffffff;
var mat:ColorMaterial = new ColorMaterial(color);
mat.lightPicker = new StaticLightPicker([light]);
mat.gloss = 2;
var bone:ObjectContainer3D = new ObjectContainer3D();
var mesh:Mesh = new Mesh(new ConeGeometry(radius, length, 8, 1), mat);
mesh.y = length/2;
bone.addChild(mesh);
return bone;
}
private function toFixed(number:Number, factor:int):Number
{
return Math.round(number * factor) / factor;
}
//------ Event Handlers ------
private var lastPosition:Vector3D = new Vector3D();
private function onEnterFrame(e:Event):void
{
if (_moving) {
_cameraController.panAngle = 0.3 * (stage.mouseX - lastMouseX) + lastPanAngle;
_cameraController.tiltAngle = 0.3 * (stage.mouseY - lastMouseY) + lastTiltAngle;
}
if(up > 0 || down > 0 || left > 0 || right > 0 || front > 0 || back > 0) {
var position:Vector3D = _targetObject.position;
position.x += (-left + right);
position.y += (up - down);
position.z += (front - back);
if( !position.equals(lastPosition) )
{
_targetObject.position = position;
this.calculateIK( position );
}
lastPosition.copyFrom(position);
}
_view.render();
}
private var lastPanAngle:Number, lastTiltAngle:Number;
private var lastMouseX:Number, lastMouseY:Number;
private var _moving:Boolean;
private function onMouseDown(event:MouseEvent):void
{
lastPanAngle = _cameraController.panAngle;
lastTiltAngle = _cameraController.tiltAngle;
lastMouseX = stage.mouseX;
lastMouseY = stage.mouseY;
_moving = true;
}
private function onMouseUp(event:MouseEvent):void
{
_moving = false;
}
private function onMouseWheel(event:MouseEvent):void
{
_cameraController.distance += event.delta * -10;
}
private function onResize(event:Event = null):void
{
_view.width = stage.stageWidth;
_view.height = stage.stageHeight;
}
private var up:int, down:int, left:int, right:int;
private var front:int, back:int;
private function onKeyDown(event:KeyboardEvent):void
{
switch(event.keyCode) {
case Keyboard.UP:
up = 1;
break;
case Keyboard.DOWN:
down = 1;
break;
case Keyboard.LEFT:
case Keyboard.A:
left = 1;
break;
case Keyboard.RIGHT:
case Keyboard.D:
right = 1;
break;
case Keyboard.W:
front = 1;
break;
case Keyboard.S:
back = 1;
break;
case Keyboard.C:
_view.renderer.swapBackBuffer = false;
_view.render();
_view.stage3DProxy.context3D.drawToBitmapData(source);
_view.renderer.swapBackBuffer = true;
break;
}
}
private function onKeyUp(event:KeyboardEvent):void
{
switch(event.keyCode) {
case Keyboard.UP:
up = 0;
break;
case Keyboard.DOWN:
down = 0;
break;
case Keyboard.LEFT:
case Keyboard.A:
left = 0;
break;
case Keyboard.RIGHT:
case Keyboard.D:
right = 0;
break;
case Keyboard.W:
front = 0;
break;
case Keyboard.S:
back = 0;
break;
}
}
}
}
import away3d.containers.ObjectContainer3D;
import away3d.core.math.Quaternion;
import flash.geom.Matrix3D;
import flash.geom.Vector3D;
internal class JointObject
{
public var position:Vector3D;
public var rotation:Quaternion ;
public var parent:int = -1;
public var relativePosition:Vector3D;
public var display:ObjectContainer3D;
public function JointObject(position:Vector3D=null, rotation:Quaternion=null)
{
this.position = position ? position : new Vector3D();
this.rotation = rotation ? rotation : new Quaternion();
}
public function setDisplay(display:ObjectContainer3D):JointObject
{
this.display = display;
return this;
}
public function setParent(index:int):JointObject
{
this.parent = index;
return this;
}
public function setRelativePosition(position:Vector3D):JointObject
{
this.relativePosition = position;
return this;
}
public function updateDisplay():void
{
var mtx:Matrix3D = rotation.toMatrix3D();
mtx.position = position;
display.transform = mtx;
}
}