/** * OimoPhysics DEV 1.1.0a * @author Saharan / http://el-ement.com/ * * Oimo.js 2014 * @author LoTh / http://3dflashlo.wordpress.com/ */ var OIMO = { REVISION: 'DEV.1.1.2a' }; OIMO.SHAPE_SPHERE = 0x1; OIMO.SHAPE_BOX = 0x2; OIMO.WORLD_SCALE = 1; OIMO.INV_SCALE = 1; OIMO.TO_RAD = Math.PI / 180; OIMO.TwoPI = 2.0 * Math.PI; // Global identification of next shape. // This will be incremented every time a shape is created. OIMO.nextID = 0; var OIMO_ARRAY_TYPE; if(!OIMO_ARRAY_TYPE) { OIMO_ARRAY_TYPE = typeof Float32Array !== 'undefined' ? Float32Array : Array; } /** * The class of physical computing world. * You must be added to the world physical all computing objects * @author saharan */ OIMO.World = function(TimeStep, BroadPhaseType, Iterations, NoStat){ // The time between each step this.timeStep= TimeStep || 1/60; // The number of iterations for constraint solvers. this.numIterations = Iterations || 8; // It is a wide-area collision judgment that is used in order to reduce as much as possible a detailed collision judgment. var broadPhaseType = BroadPhaseType || 2; switch(broadPhaseType){ case 1: this.broadPhase=new OIMO.BruteForceBroadPhase(); break; case 2: default: this.broadPhase=new OIMO.SAPBroadPhase(); break; case 3: this.broadPhase=new OIMO.DBVTBroadPhase(); break; } // Whether the constraints randomizer is enabled or not. this.enableRandomizer = true; this.isNoStat = NoStat || false; // The rigid body list this.rigidBodies=null; // number of rigid body this.numRigidBodies=0; // The contact list this.contacts=null; this.unusedContacts=null; // The number of contact this.numContacts=0; // The number of contact points this.numContactPoints=0; // The joint list this.joints=null; // The number of joints. this.numJoints=0; // The number of simulation islands. this.numIslands=0; // The gravity in the world. this.gravity=new OIMO.Vec3(0,-9.80665,0); // This is the detailed information of the performance. this.performance = null; if(!this.isNoStat) this.performance = new OIMO.Performance(this); var numShapeTypes=3; this.detectors=[]; this.detectors.length = numShapeTypes; var i=numShapeTypes; while(i--){ this.detectors[i]=[]; this.detectors[i].length = numShapeTypes; } this.detectors[OIMO.SHAPE_SPHERE][OIMO.SHAPE_SPHERE]=new OIMO.SphereSphereCollisionDetector(); this.detectors[OIMO.SHAPE_SPHERE][OIMO.SHAPE_BOX]=new OIMO.SphereBoxCollisionDetector(false); this.detectors[OIMO.SHAPE_BOX][OIMO.SHAPE_SPHERE]=new OIMO.SphereBoxCollisionDetector(true); this.detectors[OIMO.SHAPE_BOX][OIMO.SHAPE_BOX]=new OIMO.BoxBoxCollisionDetector(); this.randX=65535; this.randA=98765; this.randB=123456789; this.maxIslandRigidBodies=64; this.islandRigidBodies=[]; this.islandRigidBodies.length = this.maxIslandRigidBodies; this.islandStack=[]; this.islandStack.length = this.maxIslandRigidBodies; this.maxIslandConstraints=128; this.islandConstraints=[]; this.islandConstraints.length = this.maxIslandConstraints; }; OIMO.World.prototype = { constructor: OIMO.World, /** * Reset the randomizer and remove all rigid bodies, shapes, joints and any object from the world. */ clear:function(){ this.randX=65535; while(this.joints!==null){ this.removeJoint(this.joints); } while(this.contacts!==null){ this.removeContact(this.contacts); } while(this.rigidBodies!==null){ this.removeRigidBody(this.rigidBodies); } OIMO.nextID=0; }, /** * I'll add a rigid body to the world. * Rigid body that has been added will be the operands of each step. * @param rigidBody Rigid body that you want to add */ addRigidBody:function(rigidBody){ if(rigidBody.parent){ throw new Error("It is not possible to be added to more than one world one of the rigid body"); } rigidBody.parent=this; rigidBody.awake(); for(var shape=rigidBody.shapes; shape!==null; shape=shape.next){ this.addShape(shape); } if(this.rigidBodies!==null)(this.rigidBodies.prev=rigidBody).next=this.rigidBodies; this.rigidBodies = rigidBody; this.numRigidBodies++; }, /** * I will remove the rigid body from the world. * Rigid body that has been deleted is excluded from the calculation on a step-by-step basis. * @param rigidBody Rigid body to be removed */ removeRigidBody:function(rigidBody){ var remove=rigidBody; if(remove.parent!==this)return; remove.awake(); var js=remove.jointLink; while(js!=null){ var joint=js.joint; js=js.next; this.removeJoint(joint); } for(var shape=rigidBody.shapes; shape!==null; shape=shape.next){ this.removeShape(shape); } var prev=remove.prev; var next=remove.next; if(prev!==null) prev.next=next; if(next!==null) next.prev=prev; if(this.rigidBodies==remove) this.rigidBodies=next; remove.prev=null; remove.next=null; remove.parent=null; this.numRigidBodies--; }, getByName:function(name){ var result = null; var body=this.rigidBodies; while(body!==null){ if(body.name!== " " && body.name === name) result = body; body=body.next; } var joint=this.joints; while(joint!==null){ if(joint.name!== "" && joint.name === name) result = joint; joint=joint.next; } return result; }, /** * I'll add a shape to the world.. * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world, * Shape will be added to the world automatically, please do not call from outside this method. * @param shape Shape you want to add */ addShape:function(shape){ if(!shape.parent || !shape.parent.parent){ throw new Error("It is not possible to be added alone to shape world"); } shape.proxy = this.broadPhase.createProxy(shape); shape.updateProxy(); this.broadPhase.addProxy(shape.proxy); }, /** * I will remove the shape from the world. * Add to the rigid world, and if you add a shape to a rigid body that has been added to the world, * Shape will be added to the world automatically, please do not call from outside this method. * @param shape Shape you want to delete */ removeShape:function(shape){ this.broadPhase.removeProxy(shape.proxy); shape.proxy=null; }, /** * I'll add a joint to the world. * Joint that has been added will be the operands of each step. * @param shape Joint to be added */ addJoint:function(joint){ if(joint.parent){ throw new Error("It is not possible to be added to more than one world one of the joint"); } if(this.joints!=null)(this.joints.prev=joint).next=this.joints; this.joints=joint; joint.parent=this; this.numJoints++; joint.awake(); joint.attach(); }, /** * I will remove the joint from the world. * Joint that has been added will be the operands of each step. * @param shape Joint to be deleted */ removeJoint:function(joint){ var remove=joint; var prev=remove.prev; var next=remove.next; if(prev!==null)prev.next=next; if(next!==null)next.prev=prev; if(this.joints==remove)this.joints=next; remove.prev=null; remove.next=null; this.numJoints--; remove.awake(); remove.detach(); remove.parent=null; }, worldscale:function(scale){ OIMO.WORLD_SCALE = scale || 100; OIMO.INV_SCALE = 1/OIMO.WORLD_SCALE; }, /** * I will proceed only time step seconds time of World. */ step:function(){ var time0, time1, time2, time3; if(!this.isNoStat) time0=Date.now(); var body=this.rigidBodies; while(body!==null){ body.addedToIsland=false; if(body.sleeping){ if( body.linearVelocity.testZero() || body.position.testDiff(body.sleepPosition) || body.orientation.testDiff(body.sleepOrientation)){ body.awake(); } // awake the body /*var lv=body.linearVelocity; var av=body.linearVelocity; var p=body.position; var sp=body.sleepPosition; var o=body.orientation; var so=body.sleepOrientation; if( lv.x!==0 || lv.y!==0 || lv.z!==0 || av.x!==0 || av.y!==0 || av.z!==0 || p.x!==sp.x || p.y!==sp.y || p.z!==sp.z || o.s!==so.s || o.x!==so.x || o.y!==so.y || o.z!==so.z ){ body.awake(); }*/ } body=body.next; } //------------------------------------------------------ // UPDATE CONTACT //------------------------------------------------------ // broad phase if(!this.isNoStat) time1=Date.now(); this.broadPhase.detectPairs(); var pairs=this.broadPhase.pairs; var numPairs=this.broadPhase.numPairs; var i = numPairs; //do{ while(i--){ //for(var i=0, l=numPairs; i0); if(!this.isNoStat){ time2=Date.now(); this.performance.broadPhaseTime=time2-time1; } // update & narrow phase this.numContactPoints=0; contact=this.contacts; while(contact!==null){ if(!contact.persisting){ var aabb1=contact.shape1.aabb; var aabb2=contact.shape2.aabb; if( aabb1.minX>aabb2.maxX || aabb1.maxXaabb2.maxY || aabb1.maxYaabb2.maxZ || aabb1.maxZ0.5){ base.sleep(); }else{ base.updatePosition(this.timeStep); } }else{ base.sleepTime=0; base.updatePosition(this.timeStep); } this.numIslands++; continue; } var islandNumRigidBodies=0; var islandNumConstraints=0; var stackCount=1; // add rigid body to stack this.islandStack[0]=base; base.addedToIsland=true; // build an island do{ // get rigid body from stack body=this.islandStack[--stackCount]; this.islandStack[stackCount]=null; body.sleeping=false; // add rigid body to the island this.islandRigidBodies[islandNumRigidBodies++]=body; if(body.isStatic){ continue; } // search connections for(var cs=body.contactLink; cs!==null; cs=cs.next){ var contact=cs.contact; constraint=contact.constraint; if(constraint.addedToIsland||!contact.touching){ continue;// ignore } // add constraint to the island this.islandConstraints[islandNumConstraints++]=constraint; constraint.addedToIsland=true; var next=cs.body; if(next.addedToIsland){ continue; } // add rigid body to stack this.islandStack[stackCount++]=next; next.addedToIsland=true; } for(var js=body.jointLink; js!==null; js=js.next){ constraint=js.joint; if(constraint.addedToIsland){ continue;// ignore } // add constraint to the island this.islandConstraints[islandNumConstraints++]=constraint; constraint.addedToIsland=true; next=js.body; if(next.addedToIsland || !next.isDynamic){ continue; } // add rigid body to stack this.islandStack[stackCount++]=next; next.addedToIsland=true; } }while(stackCount!=0); // update velocities var gVel = new OIMO.Vec3().addTime(this.gravity, this.timeStep); /*var gx=this.gravity.x*this.timeStep; var gy=this.gravity.y*this.timeStep; var gz=this.gravity.z*this.timeStep;*/ var j = islandNumRigidBodies; while (j--){ //or(var j=0, l=islandNumRigidBodies; j0.5){ // sleep the island j = islandNumRigidBodies; while(j--){ //for(j=0, l=islandNumRigidBodies;j0.04)return false; if(body.angularVelocity.len()>0.25)return false; /*var v=body.linearVelocity; if(v.x*v.x+v.y*v.y+v.z*v.z>0.04){return false;} v=body.angularVelocity; if(v.x*v.x+v.y*v.y+v.z*v.z>0.25){return false;}*/ return true; } } /** * The class of rigid body. * Rigid body has the shape of a single or multiple collision processing, * I can set the parameters individually. * @author saharan */ OIMO.RigidBody = function(X,Y,Z,Rad,Ax,Ay,Az){ var rad = Rad || 0; var ax = Ax || 0; var ay = Ay || 0; var az = Az || 0; var x = X || 0; var y = Y || 0; var z = Z || 0; this.name = " "; // It is a kind of rigid body that represents the dynamic rigid body. this.BODY_DYNAMIC=0x1; // It is a kind of rigid body that represents the static rigid body. this.BODY_STATIC=0x2; // The maximum number of shapes that can be added to a one rigid. this.MAX_SHAPES=64; this.prev=null; this.next=null; // I represent the kind of rigid body. // Please do not change from the outside this variable. // If you want to change the type of rigid body, always // Please specify the type you want to set the arguments of setupMass method. this.type=0; this.massInfo= new OIMO.MassInfo(); // It is the world coordinate of the center of gravity. this.position=new OIMO.Vec3(x,y,z); this.newPosition = new OIMO.Vec3(0,0,0); this.controlPos = false; this.newOrientation = new OIMO.Quat(); this.newRotation = new OIMO.Vec3(0,0,0); this.currentRotation = new OIMO.Vec3(0,0,0); this.controlRot = false; this.controlRotInTime = false; this.orientation = this.rotationAxisToQuad(rad,ax,ay,az); /*var len=ax*ax+ay*ay+az*az; if(len>0){ len=1/Math.sqrt(len); ax*=len; ay*=len; az*=len; } var sin=Math.sin(rad*0.5); var cos=Math.cos(rad*0.5);*/ // It is a quaternion that represents the attitude. //this.orientation = new OIMO.Quat(cos,sin*ax,sin*ay,sin*az); // Is the translational velocity. this.linearVelocity = new OIMO.Vec3(); // Is the angular velocity. this.angularVelocity = new OIMO.Vec3(); // return matrix for three.js this.matrix = new OIMO.Mat44(); //-------------------------------------------- // Please do not change from the outside this variables. //-------------------------------------------- // It is a world that rigid body has been added. this.parent=null; this.contactLink=null; this.numContacts=0; // An array of shapes that are included in the rigid body. this.shapes=null; // The number of shapes that are included in the rigid body. this.numShapes=0; // It is the link array of joint that is connected to the rigid body. this.jointLink=null; // The number of joints that are connected to the rigid body. this.numJoints=0; // It is the world coordinate of the center of gravity in the sleep just before. this.sleepPosition=new OIMO.Vec3(); // It is a quaternion that represents the attitude of sleep just before. this.sleepOrientation=new OIMO.Quat(); // I will show this rigid body to determine whether it is a rigid body static. this.isStatic=false; // I indicates that this rigid body to determine whether it is a rigid body dynamic. this.isDynamic=false; // It is a rotation matrix representing the orientation. this.rotation=new OIMO.Mat33(); //-------------------------------------------- // It will be recalculated automatically from the shape, which is included. //-------------------------------------------- // This is the weight. this.mass=NaN; // It is the reciprocal of the mass. this.inverseMass=NaN; // It is the inverse of the inertia tensor in the world system. this.inverseInertia=new OIMO.Mat33(); // It is the inertia tensor in the initial state. this.localInertia=new OIMO.Mat33(); // It is the inverse of the inertia tensor in the initial state. this.inverseLocalInertia=new OIMO.Mat33(); // I indicates rigid body whether it has been added to the simulation Island. this.addedToIsland=false; // It shows how to sleep rigid body. this.allowSleep=true; // This is the time from when the rigid body at rest. this.sleepTime=0; // I shows rigid body to determine whether it is a sleep state. this.sleeping=false; } OIMO.RigidBody.prototype = { constructor: OIMO.RigidBody, /** * I'll add a shape to rigid body. * If you add a shape, please call the setupMass method to step up to the start of the next. * @param shape shape to Add */ addShape:function(shape){ if(shape.parent){ throw new Error("It is not possible that you add to the multi-rigid body the shape of one"); } if(this.shapes!=null)(this.shapes.prev=shape).next=this.shapes; this.shapes=shape; shape.parent=this; if(this.parent)this.parent.addShape(shape); this.numShapes++; }, /** * I will delete the shape from the rigid body. * If you delete a shape, please call the setupMass method to step up to the start of the next. * @param shape shape to Delete */ removeShape:function(shape){ var remove=shape; if(remove.parent!=this)return; var prev=remove.prev; var next=remove.next; if(prev!=null)prev.next=next; if(next!=null)next.prev=prev; if(this.shapes==remove)this.shapes=next; remove.prev=null; remove.next=null; remove.parent=null; if(this.parent)this.parent.removeShape(remove); this.numShapes--; }, /** * Calulates mass datas(center of gravity, mass, moment inertia, etc...). * If the parameter type is set to BODY_STATIC, the rigid body will be fixed to the space. * If the parameter adjustPosition is set to true, the shapes' relative positions and * the rigid body's position will be adjusted to the center of gravity. * @param type * @param adjustPosition */ setupMass:function(Type,AdjustPosition){ var adjustPosition = ( AdjustPosition !== undefined ) ? AdjustPosition : true; var type = Type || this.BODY_DYNAMIC; //var te = this.localInertia.elements; this.type=type; this.isDynamic=type==this.BODY_DYNAMIC; this.isStatic=type==this.BODY_STATIC; this.mass=0; this.localInertia.init(0,0,0,0,0,0,0,0,0); var te = this.localInertia.elements; // var tmpM=new OIMO.Mat33(); var tmpV=new OIMO.Vec3(); for(var shape=this.shapes;shape!=null;shape=shape.next){ shape.calculateMassInfo(this.massInfo); var shapeMass=this.massInfo.mass; var relX=shape.relativePosition.x; var relY=shape.relativePosition.y; var relZ=shape.relativePosition.z; /*tmpV.x+=relX*shapeMass; tmpV.y+=relY*shapeMass; tmpV.z+=relZ*shapeMass;*/ tmpV.addScale(shape.relativePosition, shapeMass); this.mass+=shapeMass; this.rotateInertia(shape.relativeRotation,this.massInfo.inertia,tmpM); this.localInertia.addEqual(tmpM); // add offset inertia te[0]+=shapeMass*(relY*relY+relZ*relZ); te[4]+=shapeMass*(relX*relX+relZ*relZ); te[8]+=shapeMass*(relX*relX+relY*relY); var xy=shapeMass*relX*relY; var yz=shapeMass*relY*relZ; var zx=shapeMass*relZ*relX; te[1]-=xy; te[3]-=xy; te[2]-=yz; te[6]-=yz; te[5]-=zx; te[7]-=zx; } this.inverseMass=1/this.mass; tmpV.scaleEqual(this.inverseMass); if(adjustPosition){ this.position.addEqual(tmpV); for(shape=this.shapes;shape!=null;shape=shape.next){ shape.relativePosition.subEqual(tmpV); } // subtract offset inertia relX=tmpV.x; relY=tmpV.y; relZ=tmpV.z; //var te = this.localInertia.elements; te[0]-=this.mass*(relY*relY+relZ*relZ); te[4]-=this.mass*(relX*relX+relZ*relZ); te[8]-=this.mass*(relX*relX+relY*relY); xy=this.mass*relX*relY; yz=this.mass*relY*relZ; zx=this.mass*relZ*relX; te[1]+=xy; te[3]+=xy; te[2]+=yz; te[6]+=yz; te[5]+=zx; te[7]+=zx; } this.inverseLocalInertia.invert(this.localInertia); if(type==this.BODY_STATIC){ this.inverseMass=0; this.inverseLocalInertia.init(0,0,0,0,0,0,0,0,0); } this.syncShapes(); this.awake(); }, /** * Awake the rigid body. */ awake:function(){ if(!this.allowSleep||!this.sleeping)return; this.sleeping=false; this.sleepTime=0; // awake connected constraints var cs=this.contactLink; while(cs!=null){ cs.body.sleepTime=0; cs.body.sleeping=false; cs=cs.next; } var js=this.jointLink; while(js!=null){ js.body.sleepTime=0; js.body.sleeping=false; js=js.next; } for(var shape=this.shapes;shape!=null;shape=shape.next){ shape.updateProxy(); } }, /** * Sleep the rigid body. */ sleep:function(){ if(!this.allowSleep||this.sleeping)return; this.linearVelocity.init(); this.angularVelocity.init(); this.sleepPosition.copy(this.position); this.sleepOrientation.copy(this.orientation); /*this.linearVelocity.x=0; this.linearVelocity.y=0; this.linearVelocity.z=0; this.angularVelocity.x=0; this.angularVelocity.y=0; this.angularVelocity.z=0; this.sleepPosition.x=this.position.x; this.sleepPosition.y=this.position.y; this.sleepPosition.z=this.position.z;*/ /*this.sleepOrientation.s=this.orientation.s; this.sleepOrientation.x=this.orientation.x; this.sleepOrientation.y=this.orientation.y; this.sleepOrientation.z=this.orientation.z;*/ this.sleepTime=0; this.sleeping=true; for(var shape=this.shapes;shape!=null;shape=shape.next){ shape.updateProxy(); } }, /** * Get whether the rigid body has not any connection with others. * @return */ isLonely:function(){ return this.numJoints==0&&this.numContacts==0; }, /** * The time integration of the motion of a rigid body, you can update the information such as the shape. * This method is invoked automatically when calling the step of the World, * There is no need to call from outside usually. * @param timeStep time */ updatePosition:function(timeStep){ switch(this.type){ case this.BODY_STATIC: this.linearVelocity.init(); this.angularVelocity.init(); // ONLY FOR TEST if(this.controlPos){ this.position.copy(this.newPosition); this.controlPos = false; } if(this.controlRot){ this.orientation.copy(this.newOrientation); this.controlRot = false; } /*this.linearVelocity.x=0; this.linearVelocity.y=0; this.linearVelocity.z=0; this.angularVelocity.x=0; this.angularVelocity.y=0; this.angularVelocity.z=0;*/ break; case this.BODY_DYNAMIC: if(this.controlPos){ this.angularVelocity.init(); this.linearVelocity.init(); this.linearVelocity.x = (this.newPosition.x - this.position.x)/timeStep; this.linearVelocity.y = (this.newPosition.y - this.position.y)/timeStep; this.linearVelocity.z = (this.newPosition.z - this.position.z)/timeStep; this.controlPos = false; } if(this.controlRot){ this.angularVelocity.init(); this.orientation.copy(this.newOrientation); //var t=timeStep//*0.5; //var q = new OIMO.Quat(); //q.sub(this.newOrientation, this.orientation); //q.normalize(q); /*q.s = (this.newOrientation.s - this.orientation.s)/t; q.x = (this.newOrientation.x - this.orientation.x)/t; q.y = (this.newOrientation.y - this.orientation.y)/t; q.z = (this.newOrientation.z - this.orientation.z)/t;*/ //this.angularVelocity.applyQuaternion(q); //this.angularVelocity.x = this.angularVelocity.x/t; //this.angularVelocity.y = this.angularVelocity.y/t; //this.angularVelocity.z = this.angularVelocity.z/t; this.controlRot = false; } this.position.addTime(this.linearVelocity, timeStep); this.orientation.addTime(this.angularVelocity, timeStep); break; default: throw new Error("Invalid type."); } this.syncShapes(); }, rotateInertia:function(rot,inertia,out){ var tm1 = rot.elements; var tm2 = inertia.elements; var a0 = tm1[0], a3 = tm1[3], a6 = tm1[6]; var a1 = tm1[1], a4 = tm1[4], a7 = tm1[7]; var a2 = tm1[2], a5 = tm1[5], a8 = tm1[8]; var b0 = tm2[0], b3 = tm2[3], b6 = tm2[6]; var b1 = tm2[1], b4 = tm2[4], b7 = tm2[7]; var b2 = tm2[2], b5 = tm2[5], b8 = tm2[8]; var e00 = a0*b0 + a1*b3 + a2*b6; var e01 = a0*b1 + a1*b4 + a2*b7; var e02 = a0*b2 + a1*b5 + a2*b8; var e10 = a3*b0 + a4*b3 + a5*b6; var e11 = a3*b1 + a4*b4 + a5*b7; var e12 = a3*b2 + a4*b5 + a5*b8; var e20 = a6*b0 + a7*b3 + a8*b6; var e21 = a6*b1 + a7*b4 + a8*b7; var e22 = a6*b2 + a7*b5 + a8*b8; var oe = out.elements; oe[0] = e00*a0 + e01*a1 + e02*a2; oe[1] = e00*a3 + e01*a4 + e02*a5; oe[2] = e00*a6 + e01*a7 + e02*a8; oe[3] = e10*a0 + e11*a1 + e12*a2; oe[4] = e10*a3 + e11*a4 + e12*a5; oe[5] = e10*a6 + e11*a7 + e12*a8; oe[6] = e20*a0 + e21*a1 + e22*a2; oe[7] = e20*a3 + e21*a4 + e22*a5; oe[8] = e20*a6 + e21*a7 + e22*a8; }, syncShapes:function(){ var s=this.orientation.s; var x=this.orientation.x; var y=this.orientation.y; var z=this.orientation.z; var x2=2*x; var y2=2*y; var z2=2*z; var xx=x*x2; var yy=y*y2; var zz=z*z2; var xy=x*y2; var yz=y*z2; var xz=x*z2; var sx=s*x2; var sy=s*y2; var sz=s*z2; var tr = this.rotation.elements; tr[0]=1-yy-zz; tr[1]=xy-sz; tr[2]=xz+sy; tr[3]=xy+sz; tr[4]=1-xx-zz; tr[5]=yz-sx; tr[6]=xz-sy; tr[7]=yz+sx; tr[8]=1-xx-yy; this.rotateInertia(this.rotation,this.inverseLocalInertia,this.inverseInertia); for(var shape=this.shapes;shape!=null;shape=shape.next){ //var relPos=shape.relativePosition; //var relRot=shape.relativeRotation; //var rot=shape.rotation; /*var lx=relPos.x; var ly=relPos.y; var lz=relPos.z; shape.position.x=this.position.x+lx*tr[0]+ly*tr[1]+lz*tr[2]; shape.position.y=this.position.y+lx*tr[3]+ly*tr[4]+lz*tr[5]; shape.position.z=this.position.z+lx*tr[6]+ly*tr[7]+lz*tr[8];*/ shape.position.mul(this.position,shape.relativePosition,this.rotation); shape.rotation.mul(shape.relativeRotation,this.rotation); shape.updateProxy(); } }, applyImpulse:function(position,force){ this.linearVelocity.addScale(force, this.inverseMass); /*this.linearVelocity.x+=force.x*this.inverseMass; this.linearVelocity.y+=force.y*this.inverseMass; this.linearVelocity.z+=force.z*this.inverseMass;*/ var rel=new OIMO.Vec3(); rel.sub(position,this.position).cross(rel,force).mulMat(this.inverseInertia,rel); this.angularVelocity.addEqual(rel); /*this.angularVelocity.x+=rel.x; this.angularVelocity.y+=rel.y; this.angularVelocity.z+=rel.z;*/ }, //--------------------------------------------- // // FOR THREE JS // //--------------------------------------------- rotationVectToQuad: function(rot){ var r = OIMO.EulerToAxis( rot.x * OIMO.TO_RAD, rot.y * OIMO.TO_RAD, rot.z * OIMO.TO_RAD ); return this.rotationAxisToQuad(r[0], r[1], r[2], r[3]); }, rotationAxisToQuad: function(rad, ax, ay, az){ // in radian var len=ax*ax+ay*ay+az*az; if(len>0){ len=1/Math.sqrt(len); ax*=len; ay*=len; az*=len; } var sin=Math.sin(rad*0.5); var cos=Math.cos(rad*0.5); return new OIMO.Quat(cos,sin*ax,sin*ay,sin*az); }, //--------------------------------------------- // SET DYNAMIQUE POSITION AND ROTATION //--------------------------------------------- setPosition:function(pos){ this.newPosition.init(pos.x*OIMO.INV_SCALE,pos.y*OIMO.INV_SCALE,pos.z*OIMO.INV_SCALE); this.controlPos = true; }, setQuaternion:function(q){ //if(this.type == this.BODY_STATIC)this.orientation.init(q.w,q.x,q.y,q.z); this.newOrientation.init(q.w,q.x,q.y,q.z); this.controlRot = true; }, setRotation:function(rot){ this.newOrientation = this.rotationVectToQuad(rot); this.controlRot = true; }, //--------------------------------------------- // RESET DYNAMIQUE POSITION AND ROTATION //--------------------------------------------- resetPosition:function(x,y,z){ this.linearVelocity.init(); this.angularVelocity.init(); this.position.init(x*OIMO.INV_SCALE,y*OIMO.INV_SCALE,z*OIMO.INV_SCALE); this.awake(); }, resetQuaternion:function(q){ this.angularVelocity.init(); this.orientation = new OIMO.Quat(q.w,q.x,q.y,q.z); this.awake(); }, resetRotation:function(x,y,z){ this.angularVelocity.init(); this.orientation = this.rotationVectToQuad(new OIMO.Vec3(x,y,z)); this.awake(); }, //--------------------------------------------- // GET POSITION AND ROTATION //--------------------------------------------- getPosition:function(){ return new OIMO.Vec3().scale(this.position, OIMO.WORLD_SCALE); }, getRotation:function(){ return new OIMO.Euler().setFromRotationMatrix(this.rotation); }, getQuaternion:function(){ return new OIMO.Quaternion().setFromRotationMatrix(this.rotation); }, getMatrix:function(){ var m = this.matrix.elements; var r,p; if(!this.sleeping){ // rotation matrix r = this.rotation.elements; m[0] = r[0]; m[1] = r[3]; m[2] = r[6]; m[3] = 0; m[4] = r[1]; m[5] = r[4]; m[6] = r[7]; m[7] = 0; m[8] = r[2]; m[9] = r[5]; m[10] = r[8]; m[11] = 0; // position matrix p = this.position; m[12] = p.x*OIMO.WORLD_SCALE; m[13] = p.y*OIMO.WORLD_SCALE; m[14] = p.z*OIMO.WORLD_SCALE; // sleep or not ? m[15] = 0; } else { m[15] = 1; } return m; } } /** * The main class of body. * is for simplify creation process and data access of rigidRody * Rigid body has the shape of a single or multiple collision processing, * all setting in object * * @author loth */ OIMO.Body = function(Obj){ var obj = Obj || {}; if(!obj.world) return; // the world where i am this.parent = obj.world; // Yep my name this.name = obj.name || ''; // I'm dynamique or not var move = obj.move || false; // I can sleep or not var noSleep = obj.noSleep || false; // My start position var p = obj.pos || [0,0,0]; p = p.map(function(x) { return x * OIMO.INV_SCALE; }); // My size var s = obj.size || [1,1,1]; s = s.map(function(x) { return x * OIMO.INV_SCALE; }); // My rotation in degre var rot = obj.rot || [0,0,0]; rot = rot.map(function(x) { return x * OIMO.TO_RAD; }); var r = []; for (var i=0; i0){ //shapes[i].position.init(p[0]+p[n+0], p[1]+p[n+1], p[2]+p[n+2] ); shapes[i].relativePosition = new OIMO.Vec3( p[n+0], p[n+1], p[n+2] ); if(r[n2+0]) shapes[i].relativeRotation = [ r[n2+0], r[n2+1], r[n2+2], r[n2+3] ]; } } // I'm static or i move if(move){ if(obj.massPos || obj.massRot)this.body.setupMass(0x1, false); else this.body.setupMass(0x1, true); if(noSleep) this.body.allowSleep = false; else this.body.allowSleep = true; } else { this.body.setupMass(0x2); } this.body.name = this.name; this.sleeping = this.body.sleeping; // finaly add to physics world this.parent.addRigidBody(this.body); } OIMO.Body.prototype = { constructor: OIMO.Body, // SET setPosition:function(pos){ this.body.setPosition(pos); }, setQuaternion:function(q){ this.body.setQuaternion(q); }, setRotation:function(rot){ this.body.setRotation(rot); }, // GET getPosition:function(){ return this.body.getPosition(); }, getRotation:function(){ return this.body.getRotation(); }, getQuaternion:function(){ return this.body.getQuaternion(); }, getMatrix:function(){ return this.body.getMatrix(); }, getSleep:function(){ return this.body.sleeping; }, // RESET resetPosition:function(x,y,z){ this.body.resetPosition(x,y,z); }, resetRotation:function(x,y,z){ this.body.resetRotation(x,y,z); }, // force wakeup awake:function(){ this.body.awake(); }, // remove rigidbody remove:function(){ this.parent.removeRigidBody(this.body); }, // test if this object hit another checkContact:function(name){ this.parent.checkContact(this.name, name); } } /** * The main class of link. * is for simplify creation process and data access of Joint * all setting in object * * @author loth */ OIMO.Link = function(Obj){ var obj = Obj || {}; if(!obj.world) return; // the world where i am this.parent = obj.world; this.name = obj.name || ''; var type = obj.type || "jointHinge"; var axe1 = obj.axe1 || [1,0,0]; var axe2 = obj.axe2 || [1,0,0]; var pos1 = obj.pos1 || [0,0,0]; var pos2 = obj.pos2 || [0,0,0]; pos1 = pos1.map(function(x){ return x * OIMO.INV_SCALE; }); pos2 = pos2.map(function(x){ return x * OIMO.INV_SCALE; }); var min, max; if(type==="jointDistance"){ min = obj.min || 0; max = obj.max || 10; min = min*OIMO.INV_SCALE; max = max*OIMO.INV_SCALE; }else{ min = obj.min || 57.29578; max = obj.max || 0; min = min*OIMO.TO_RAD; max = max*OIMO.TO_RAD; } var limit = obj.limit || null; var spring = obj.spring || null; var motor = obj.motor || null; // joint setting var jc = new OIMO.JointConfig(); jc.allowCollision = obj.collision || false;; jc.localAxis1.init(axe1[0], axe1[1], axe1[2]); jc.localAxis2.init(axe2[0], axe2[1], axe2[2]); jc.localAnchorPoint1.init(pos1[0], pos1[1], pos1[2]); jc.localAnchorPoint2.init(pos2[0], pos2[1], pos2[2]); if (typeof obj.body1 == 'string' || obj.body1 instanceof String) obj.body1 = obj.world.getByName(obj.body1); if (typeof obj.body2 == 'string' || obj.body2 instanceof String) obj.body2 = obj.world.getByName(obj.body2); jc.body1 = obj.body1; jc.body2 = obj.body2; switch(type){ case "jointDistance": this.joint = new OIMO.DistanceJoint(jc, min, max); if(spring !== null) this.joint.limitMotor.setSpring(spring[0], spring[1]); if(motor !== null) this.joint.limitMotor.setSpring(motor[0], motor[1]); break; case "jointHinge": this.joint = new OIMO.HingeJoint(jc, min, max); if(spring !== null) this.joint.limitMotor.setSpring(spring[0], spring[1]);// soften the joint ex: 100, 0.2 if(motor !== null) this.joint.limitMotor.setSpring(motor[0], motor[1]); break; case "jointPrisme": this.joint = new OIMO.PrismaticJoint(jc, min, max); break; case "jointSlide": this.joint = new OIMO.SliderJoint(jc, min, max); break; case "jointBall": this.joint = new OIMO.BallAndSocketJoint(jc); break; case "jointWheel": this.joint = new OIMO.WheelJoint(jc); if(limit !== null) this.joint.rotationalLimitMotor1.setLimit(limit[0], limit[1]); if(spring !== null) this.joint.rotationalLimitMotor1.setSpring(spring[0], spring[1]); if(motor !== null) this.joint.rotationalLimitMotor1.setSpring(motor[0], motor[1]); break; } this.joint.name = this.name; // finaly add to physics world this.parent.addJoint(this.joint); } OIMO.Link.prototype = { constructor: OIMO.Link, getPosition:function(){ // array of two vect3 [point1, point2] return this.joint.getPosition(); }, getMatrix:function(){ return this.joint.getMatrix(); }, // remove joint remove:function(){ this.parent.removeJoint(this.joint); }, // force wakeup linked body awake:function(){ this.joint.awake(); } } OIMO.Performance = function(world){ this.parent = world; this.infos = new OIMO_ARRAY_TYPE(13); this.f = [0,0,0]; this.fps=0; this.broadPhaseTime=0; this.narrowPhaseTime=0; this.solvingTime=0; this.updatingTime=0; this.totalTime=0; } OIMO.Performance.prototype = { upfps : function(){ this.f[1] = Date.now(); if (this.f[1]-1000>this.f[0]){ this.f[0]=this.f[1]; this.fps=this.f[2]; this.f[2]=0; } this.f[2]++; }, show : function(){ var info =[ "Oimo.js DEV.1.1.2a

", "FPS: " + this.fps +" fps

", "rigidbody "+this.parent.numRigidBodies+"
", "contact   "+this.parent.numContacts+"
", "paircheck "+this.parent.broadPhase.numPairChecks+"
", "contact   "+this.parent.numContactPoints+"
", "island    "+this.parent.numIslands +"

", "Time in ms
", "broad-phase  "+this.broadPhaseTime + "
", "narrow-phase "+this.narrowPhaseTime + "
", "solving      "+this.solvingTime + "
", "updating     "+this.updatingTime + "
", "total        "+this.totalTime ].join("\n"); return info; }, toArray : function(){ this.infos[0] = this.parent.broadPhase.types; this.infos[1] = this.parent.numRigidBodies; this.infos[2] = this.parent.numContacts; this.infos[3] = this.parent.broadPhase.numPairChecks; this.infos[4] = this.parent.numContactPoints; this.infos[5] = this.parent.numIslands; this.infos[6] = this.broadPhaseTime; this.infos[7] = this.narrowPhaseTime; this.infos[8] = this.solvingTime; this.infos[9] = this.updatingTime; this.infos[10] = this.totalTime; this.infos[11] = this.fps; return this.infos; } } OIMO.Mat44 = function(n11, n12, n13, n14, n21, n22, n23, n24, n31, n32, n33, n34, n41, n42, n43, n44){ this.elements = new OIMO_ARRAY_TYPE(16); var te = this.elements; te[0] = ( n11 !== undefined ) ? n11 : 1; te[4] = n12 || 0; te[8] = n13 || 0; te[12] = n14 || 0; te[1] = n21 || 0; te[5] = ( n22 !== undefined ) ? n22 : 1; te[9] = n23 || 0; te[13] = n24 || 0; te[2] = n31 || 0; te[6] = n32 || 0; te[10] = ( n33 !== undefined ) ? n33 : 1; te[14] = n34 || 0; te[3] = n41 || 0; te[7] = n42 || 0; te[11] = n43 || 0; te[15] = ( n44 !== undefined ) ? n44 : 1; }; OIMO.Mat44.prototype = { constructor: OIMO.Mat44, set: function ( n11, n12, n13, n14, n21, n22, n23, n24, n31, n32, n33, n34, n41, n42, n43, n44 ) { var te = this.elements; te[0] = n11; te[4] = n12; te[8] = n13; te[12] = n14; te[1] = n21; te[5] = n22; te[9] = n23; te[13] = n24; te[2] = n31; te[6] = n32; te[10] = n33; te[14] = n34; te[3] = n41; te[7] = n42; te[11] = n43; te[15] = n44; return this; }/*, extractRotation: function () { var v1 = new THREE.Vector3(); return function ( m ) { var te = this.elements; var me = m.elements; var scaleX = 1 / v1.set( me[ 0 ], me[ 1 ], me[ 2 ] ).length(); var scaleY = 1 / v1.set( me[ 4 ], me[ 5 ], me[ 6 ] ).length(); var scaleZ = 1 / v1.set( me[ 8 ], me[ 9 ], me[ 10 ] ).length(); te[ 0 ] = me[ 0 ] * scaleX; te[ 1 ] = me[ 1 ] * scaleX; te[ 2 ] = me[ 2 ] * scaleX; te[ 4 ] = me[ 4 ] * scaleY; te[ 5 ] = me[ 5 ] * scaleY; te[ 6 ] = me[ 6 ] * scaleY; te[ 8 ] = me[ 8 ] * scaleZ; te[ 9 ] = me[ 9 ] * scaleZ; te[ 10 ] = me[ 10 ] * scaleZ; return this; }; }() */ } OIMO.Mat33 = function(e00,e01,e02,e10,e11,e12,e20,e21,e22){ this.elements = new OIMO_ARRAY_TYPE(9); var te = this.elements; this.init( ( e00 !== undefined ) ? e00 : 1, e01 || 0, e02 || 0, e10 || 0, ( e11 !== undefined ) ? e11 : 1, e12 || 0, e20 || 0, e21 || 0, ( e22 !== undefined ) ? e22 : 1 ); }; OIMO.Mat33.prototype = { constructor: OIMO.Mat33, init: function(e00,e01,e02,e10,e11,e12,e20,e21,e22){ var te = this.elements; te[0] = e00; te[1] = e01; te[2] = e02; te[3] = e10; te[4] = e11; te[5] = e12; te[6] = e20; te[7] = e21; te[8] = e22; return this; }, add: function(m1,m2){ var te = this.elements, tem1 = m1.elements, tem2 = m2.elements; te[0] = tem1[0] + tem2[0]; te[1] = tem1[1] + tem2[1]; te[2] = tem1[2] + tem2[2]; te[3] = tem1[3] + tem2[3]; te[4] = tem1[4] + tem2[4]; te[5] = tem1[5] + tem2[5]; te[6] = tem1[6] + tem2[6]; te[7] = tem1[7] + tem2[7]; te[8] = tem1[8] + tem2[8]; return this; }, addEqual: function(m){ var te = this.elements, tem = m.elements; te[0] += tem[0]; te[1] += tem[1]; te[2] += tem[2]; te[3] += tem[3]; te[4] += tem[4]; te[5] += tem[5]; te[6] += tem[6]; te[7] += tem[7]; te[8] += tem[8]; return this; }, sub: function(m1,m2){ var te = this.elements, tem1 = m1.elements, tem2 = m2.elements; te[0] = tem1[0] - tem2[0]; te[1] = tem1[1] - tem2[1]; te[2] = tem1[2] - tem2[2]; te[3] = tem1[3] - tem2[3]; te[4] = tem1[4] - tem2[4]; te[5] = tem1[5] - tem2[5]; te[6] = tem1[6] - tem2[6]; te[7] = tem1[7] - tem2[7]; te[8] = tem1[8] - tem2[8]; return this; }, subEqual:function(m){ var te = this.elements, tem = m.elements; te[0] -= tem[0]; te[1] -= tem[1]; te[2] -= tem[2]; te[3] -= tem[3]; te[4] -= tem[4]; te[5] -= tem[5]; te[6] -= tem[6]; te[7] -= tem[7]; te[8] -= tem[8]; return this; }, scale: function(m,s){ var te = this.elements, tm = m.elements; te[0] = tm[0] * s; te[1] = tm[1] * s; te[2] = tm[2] * s; te[3] = tm[3] * s; te[4] = tm[4] * s; te[5] = tm[5] * s; te[6] = tm[6] * s; te[7] = tm[7] * s; te[8] = tm[8] * s; return this; }, scaleEqual: function(s){ var te = this.elements; te[0] *= s; te[1] *= s; te[2] *= s; te[3] *= s; te[4] *= s; te[5] *= s; te[6] *= s; te[7] *= s; te[8] *= s; return this; }, mul: function(m1,m2){ var te = this.elements, tm1 = m1.elements, tm2 = m2.elements, a0 = tm1[0], a3 = tm1[3], a6 = tm1[6], a1 = tm1[1], a4 = tm1[4], a7 = tm1[7], a2 = tm1[2], a5 = tm1[5], a8 = tm1[8], b0 = tm2[0], b3 = tm2[3], b6 = tm2[6], b1 = tm2[1], b4 = tm2[4], b7 = tm2[7], b2 = tm2[2], b5 = tm2[5], b8 = tm2[8]; te[0] = a0*b0 + a1*b3 + a2*b6; te[1] = a0*b1 + a1*b4 + a2*b7; te[2] = a0*b2 + a1*b5 + a2*b8; te[3] = a3*b0 + a4*b3 + a5*b6; te[4] = a3*b1 + a4*b4 + a5*b7; te[5] = a3*b2 + a4*b5 + a5*b8; te[6] = a6*b0 + a7*b3 + a8*b6; te[7] = a6*b1 + a7*b4 + a8*b7; te[8] = a6*b2 + a7*b5 + a8*b8; return this; }, mulScale: function(m,sx,sy,sz,Prepend){ var prepend = Prepend || false; var te = this.elements, tm = m.elements; if(prepend){ te[0] = sx*tm[0]; te[1] = sx*tm[1]; te[2] = sx*tm[2]; te[3] = sy*tm[3]; te[4] = sy*tm[4]; te[5] = sy*tm[5]; te[6] = sz*tm[6]; te[7] = sz*tm[7]; te[8] = sz*tm[8]; }else{ te[0] = tm[0]*sx; te[1] = tm[1]*sy; te[2] = tm[2]*sz; te[3] = tm[3]*sx; te[4] = tm[4]*sy; te[5] = tm[5]*sz; te[6] = tm[6]*sx; te[7] = tm[7]*sy; te[8] = tm[8]*sz; } return this; }, mulRotate: function(m,rad,ax,ay,az,Prepend){ var prepend = Prepend || false; var s=Math.sin(rad); var c=Math.cos(rad); var c1=1-c; var r00=ax*ax*c1+c; var r01=ax*ay*c1-az*s; var r02=ax*az*c1+ay*s; var r10=ay*ax*c1+az*s; var r11=ay*ay*c1+c; var r12=ay*az*c1-ax*s; var r20=az*ax*c1-ay*s; var r21=az*ay*c1+ax*s; var r22=az*az*c1+c; var tm = m.elements; var a0 = tm[0], a3 = tm[3], a6 = tm[6]; var a1 = tm[1], a4 = tm[4], a7 = tm[7]; var a2 = tm[2], a5 = tm[5], a8 = tm[8]; var te = this.elements; if(prepend){ te[0]=r00*a0+r01*a3+r02*a6; te[1]=r00*a1+r01*a4+r02*a7; te[2]=r00*a2+r01*a5+r02*a8; te[3]=r10*a0+r11*a3+r12*a6; te[4]=r10*a1+r11*a4+r12*a7; te[5]=r10*a2+r11*a5+r12*a8; te[6]=r20*a0+r21*a3+r22*a6; te[7]=r20*a1+r21*a4+r22*a7; te[8]=r20*a2+r21*a5+r22*a8; }else{ te[0]=a0*r00+a1*r10+a2*r20; te[1]=a0*r01+a1*r11+a2*r21; te[2]=a0*r02+a1*r12+a2*r22; te[3]=a3*r00+a4*r10+a5*r20; te[4]=a3*r01+a4*r11+a5*r21; te[5]=a3*r02+a4*r12+a5*r22; te[6]=a6*r00+a7*r10+a8*r20; te[7]=a6*r01+a7*r11+a8*r21; te[8]=a6*r02+a7*r12+a8*r22; } return this; }, transpose: function(m){ var te = this.elements, tm = m.elements; te[0] = tm[0]; te[1] = tm[3]; te[2] = tm[6]; te[3] = tm[1]; te[4] = tm[4]; te[5] = tm[7]; te[6] = tm[2]; te[7] = tm[5]; te[8] = tm[8]; return this; }, setQuat: function(q){ var te = this.elements, x2=2*q.x, y2=2*q.y, z2=2*q.z, xx=q.x*x2, yy=q.y*y2, zz=q.z*z2, xy=q.x*y2, yz=q.y*z2, xz=q.x*z2, sx=q.s*x2, sy=q.s*y2, sz=q.s*z2; te[0]=1-yy-zz; te[1]=xy-sz; te[2]=xz+sy; te[3]=xy+sz; te[4]=1-xx-zz; te[5]=yz-sx; te[6]=xz-sy; te[7]=yz+sx; te[8]=1-xx-yy; return this; }, invert: function(m){ var te = this.elements, tm = m.elements, a0 = tm[0], a3 = tm[3], a6 = tm[6], a1 = tm[1], a4 = tm[4], a7 = tm[7], a2 = tm[2], a5 = tm[5], a8 = tm[8], b01 = a4*a8-a7*a5, b11 = a7*a2-a1*a8, b21 = a1*a5-a4*a2, dt= a0 * (b01) + a3 * (b11) + a6 * (b21); if(dt!=0){dt=1.0/dt;} te[0] = dt*b01;//(a4*a8 - a5*a7); te[1] = dt*b11;//(a2*a7 - a1*a8); te[2] = dt*b21;//(a1*a5 - a2*a4); te[3] = dt*(a5*a6 - a3*a8); te[4] = dt*(a0*a8 - a2*a6); te[5] = dt*(a2*a3 - a0*a5); te[6] = dt*(a3*a7 - a4*a6); te[7] = dt*(a1*a6 - a0*a7); te[8] = dt*(a0*a4 - a1*a3); return this; }, copy: function(m){ var te = this.elements, tem = m.elements; te[0] = tem[0]; te[1] = tem[1]; te[2] = tem[2]; te[3] = tem[3]; te[4] = tem[4]; te[5] = tem[5]; te[6] = tem[6]; te[7] = tem[7]; te[8] = tem[8]; return this; }, toEuler: function(){ // not work !! function clamp( x ) { return Math.min( Math.max( x, -1 ), 1 ); } var te = this.elements; var m11 = te[0], m12 = te[3], m13 = te[6]; var m21 = te[1], m22 = te[4], m23 = te[7]; var m31 = te[2], m32 = te[5], m33 = te[8]; var p = new OIMO.Vec3(); var d = new OIMO.Quat(); var s; p.y = Math.asin( clamp( m13 ) ); if ( Math.abs( m13 ) < 0.99999 ) { p.x = Math.atan2( - m23, m33 ); p.z = Math.atan2( - m12, m11 ); } else { p.x = Math.atan2( m32, m22 ); p.z = 0; } return p; }, clone: function(){ var te = this.elements; return new OIMO.Mat33( te[0], te[1], te[2], te[3], te[4], te[5], te[6], te[7], te[8] ); }, toString: function(){ var te = this.elements; var text= "Mat33|"+te[0].toFixed(4)+", "+te[1].toFixed(4)+", "+te[2].toFixed(4)+"|\n"+ " |"+te[3].toFixed(4)+", "+te[4].toFixed(4)+", "+te[5].toFixed(4)+"|\n"+ " |"+te[6].toFixed(4)+", "+te[7].toFixed(4)+", "+te[8].toFixed(4)+"|" ; return text; } }; OIMO.Quat = function( s, x, y, z){ this.s=( s !== undefined ) ? s : 1; this.x=x || 0; this.y=y || 0; this.z=z || 0; }; OIMO.Quat.prototype = { constructor: OIMO.Quat, init: function(s,x,y,z){ this.s=( s !== undefined ) ? s : 1; this.x=x || 0; this.y=y || 0; this.z=z || 0; return this; }, add: function(q1,q2){ this.s=q1.s+q2.s; this.x=q1.x+q2.x; this.y=q1.y+q2.y; this.z=q1.z+q2.z; return this; }, addTime: function(v,t){ var x = v.x; var y = v.y; var z = v.z; var qs=this.s; var qx=this.x; var qy=this.y; var qz=this.z; t*=0.5; var ns=(-x*qx - y*qy - z*qz)*t; var nx=( x*qs + y*qz - z*qy)*t; var ny=(-x*qz + y*qs + z*qx)*t; var nz=( x*qy - y*qx + z*qs)*t; qs += ns; qx += nx; qy += ny; qz += nz; var s=1/Math.sqrt(qs*qs+qx*qx+qy*qy+qz*qz); this.s=qs*s; this.x=qx*s; this.y=qy*s; this.z=qz*s; return this; }, sub: function(q1,q2){ this.s=q1.s-q2.s; this.x=q1.x-q2.x; this.y=q1.y-q2.y; this.z=q1.z-q2.z; return this; }, scale: function(q,s){ this.s=q.s*s; this.x=q.x*s; this.y=q.y*s; this.z=q.z*s; return this; }, mul: function(q1,q2){ var ax = q1.x, ay = q1.y, az = q1.z, as = q1.s, bx = q2.x, by = q2.y, bz = q2.z, bs = q2.s; this.x = ax * bs + as * bx + ay * bz - az * by; this.y = ay * bs + as * by + az * bx - ax * bz; this.z = az * bs + as * bz + ax * by - ay * bx; this.s = as * bs - ax * bx - ay * by - az * bz; return this; }, arc: function(v1,v2){ var x1=v1.x; var y1=v1.y; var z1=v1.z; var x2=v2.x; var y2=v2.y; var z2=v2.z; var d=x1*x2+y1*y2+z1*z2; if(d==-1){ x2=y1*x1-z1*z1; y2=-z1*y1-x1*x1; z2=x1*z1+y1*y1; d=1/Math.sqrt(x2*x2+y2*y2+z2*z2); this.s=0; this.x=x2*d; this.y=y2*d; this.z=z2*d; return this; } var cx=y1*z2-z1*y2; var cy=z1*x2-x1*z2; var cz=x1*y2-y1*x2; this.s=Math.sqrt((1+d)*0.5); d=0.5/this.s; this.x=cx*d; this.y=cy*d; this.z=cz*d; return this; }, normalize: function(q){ var len=Math.sqrt(q.s*q.s+q.x*q.x+q.y*q.y+q.z*q.z); if(len>0){len=1/len;} this.s=q.s*len; this.x=q.x*len; this.y=q.y*len; this.z=q.z*len; return this; }, invert: function(q){ this.s=q.s; this.x=-q.x; this.y=-q.y; this.z=-q.z; return this; }, length: function(){ return Math.sqrt(this.s*this.s+this.x*this.x+this.y*this.y+this.z*this.z); }, copy: function(q){ this.s=q.s; this.x=q.x; this.y=q.y; this.z=q.z; return this; }, testDiff: function(q){ if(this.s!==q.s || this.x!==q.x || this.y!==q.y || this.z!==q.z) return true; else return false; }, clone: function(q){ return new OIMO.Quat(this.s,this.x,this.y,this.z); }, toString: function(){ return"Quat["+this.s.toFixed(4)+", ("+this.x.toFixed(4)+", "+this.y.toFixed(4)+", "+this.z.toFixed(4)+")]"; } } // for three easy export OIMO.Quaternion = function ( x, y, z, w ) { this.x = x || 0; this.y = y || 0; this.z = z || 0; this.w = ( w !== undefined ) ? w : 1; }; OIMO.Quaternion.prototype = { constructor: OIMO.Quaternion, setFromRotationMatrix: function ( m ) { // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled) var te = m.elements, m11 = te[ 0 ], m12 = te[ 1 ], m13 = te[ 2 ], m21 = te[ 3 ], m22 = te[ 4 ], m23 = te[ 5 ], m31 = te[ 6 ], m32 = te[ 7 ], m33 = te[ 8 ], trace = m11 + m22 + m33, s; if ( trace > 0 ) { s = 0.5 / Math.sqrt( trace + 1.0 ); this.w = 0.25 / s; this.x = ( m32 - m23 ) * s; this.y = ( m13 - m31 ) * s; this.z = ( m21 - m12 ) * s; } else if ( m11 > m22 && m11 > m33 ) { s = 2.0 * Math.sqrt( 1.0 + m11 - m22 - m33 ); this.w = ( m32 - m23 ) / s; this.x = 0.25 * s; this.y = ( m12 + m21 ) / s; this.z = ( m13 + m31 ) / s; } else if ( m22 > m33 ) { s = 2.0 * Math.sqrt( 1.0 + m22 - m11 - m33 ); this.w = ( m13 - m31 ) / s; this.x = ( m12 + m21 ) / s; this.y = 0.25 * s; this.z = ( m23 + m32 ) / s; } else { s = 2.0 * Math.sqrt( 1.0 + m33 - m11 - m22 ); this.w = ( m21 - m12 ) / s; this.x = ( m13 + m31 ) / s; this.y = ( m23 + m32 ) / s; this.z = 0.25 * s; } //this.onChangeCallback(); return this; } } OIMO.Vec3 = function(x,y,z){ this.x=x || 0; this.y=y || 0; this.z=z || 0; }; OIMO.Vec3.prototype = { constructor: OIMO.Vec3, init: function(x,y,z){ this.x=x || 0; this.y=y || 0; this.z=z || 0; return this; }, set: function(x,y,z){ this.x=x; this.y=y; this.z=z; return this; }, add: function(v1,v2){ this.x=v1.x+v2.x; this.y=v1.y+v2.y; this.z=v1.z+v2.z; return this; }, addEqual: function(v){ this.x+=v.x; this.y+=v.y; this.z+=v.z; return this; }, addTime: function(v, t){ this.x+=v.x*t; this.y+=v.y*t; this.z+=v.z*t; return this; }, sub: function(v1,v2){ this.x=v1.x-v2.x; this.y=v1.y-v2.y; this.z=v1.z-v2.z; return this; }, subEqual: function(v){ this.x-=v.x; this.y-=v.y; this.z-=v.z; return this; }, addScale: function(v,s){ this.x+=v.x*s; this.y+=v.y*s; this.z+=v.z*s; return this; }, scale: function(v,s){ this.x=v.x*s; this.y=v.y*s; this.z=v.z*s; return this; }, scaleEqual: function(s){ this.x*=s; this.y*=s; this.z*=s; return this; }, dot: function(v){ return this.x*v.x+this.y*v.y+this.z*v.z; }, cross: function(v1,v2){ var ax = v1.x, ay = v1.y, az = v1.z, bx = v2.x, by = v2.y, bz = v2.z; this.x = ay * bz - az * by; this.y = az * bx - ax * bz; this.z = ax * by - ay * bx; return this; }, mul: function(o, v, m){ var te = m.elements; this.x=o.x+v.x*te[0]+v.y*te[1]+v.z*te[2]; this.y=o.y+v.x*te[3]+v.y*te[4]+v.z*te[5]; this.z=o.z+v.x*te[6]+v.y*te[7]+v.z*te[8]; return this; }, mulMat: function(m,v){ var te = m.elements; var x=te[0]*v.x+te[1]*v.y+te[2]*v.z; var y=te[3]*v.x+te[4]*v.y+te[5]*v.z; var z=te[6]*v.x+te[7]*v.y+te[8]*v.z; this.x=x; this.y=y; this.z=z; return this; }, normalize: function(v){ var x = v.x, y = v.y, z = v.z; var l = x*x + y*y + z*z; if (l > 0) { l = 1 / Math.sqrt(l); this.x = x*l; this.y = y*l; this.z = z*l; } return this; }, invert: function(v){ this.x=-v.x; this.y=-v.y; this.z=-v.z; return this; }, length: function(){ var x = this.x, y = this.y, z = this.z; return Math.sqrt(x*x + y*y + z*z); }, len: function(){ var x = this.x, y = this.y, z = this.z; return x*x + y*y + z*z; }, copy: function(v){ this.x=v.x; this.y=v.y; this.z=v.z; return this; }, applyQuaternion: function ( q ) { var x = this.x; var y = this.y; var z = this.z; var qx = q.x; var qy = q.y; var qz = q.z; var qs = q.s; // calculate quat * vector var ix = qs * x + qy * z - qz * y; var iy = qs * y + qz * x - qx * z; var iz = qs * z + qx * y - qy * x; var iw = - qx * x - qy * y - qz * z; // calculate result * inverse quat this.x = ix * qs + iw * - qx + iy * - qz - iz * - qy; this.y = iy * qs + iw * - qy + iz * - qx - ix * - qz; this.z = iz * qs + iw * - qz + ix * - qy - iy * - qx; return this; }, testZero: function(){ if(this.x!==0 || this.y!==0 || this.z!==0) return true; else return false; }, testDiff: function(v){ if(this.x!==v.x || this.y!==v.y || this.z!==v.z) return true; else return false; }, clone: function(){ return new OIMO.Vec3(this.x,this.y,this.z); }, toString: function(){ return"Vec3["+this.x.toFixed(4)+", "+this.y.toFixed(4)+", "+this.z.toFixed(4)+"]"; } } OIMO.Euler = function ( x, y, z, order ) { this._x = x || 0; this._y = y || 0; this._z = z || 0; this._order = order || OIMO.Euler.DefaultOrder; }; OIMO.Euler.RotationOrders = [ 'XYZ', 'YZX', 'ZXY', 'XZY', 'YXZ', 'ZYX' ]; OIMO.Euler.DefaultOrder = 'XYZ'; OIMO.clamp = function ( x, a, b ) { return ( x < a ) ? a : ( ( x > b ) ? b : x ); } OIMO.Euler.prototype = { constructor: OIMO.Euler, _x: 0, _y: 0, _z: 0, _order: OIMO.Euler.DefaultOrder, get x () { return this._x; }, set x ( value ) { this._x = value; this.onChangeCallback(); }, get y () { return this._y; }, set y ( value ) { this._y = value; this.onChangeCallback(); }, get z () { return this._z; }, set z ( value ) { this._z = value; this.onChangeCallback(); }, get order () { return this._order; }, set order ( value ) { this._order = value; this.onChangeCallback(); }, set: function ( x, y, z, order ) { this._x = x; this._y = y; this._z = z; this._order = order || this._order; this.onChangeCallback(); return this; }, copy: function ( euler ) { this._x = euler._x; this._y = euler._y; this._z = euler._z; this._order = euler._order; this.onChangeCallback(); return this; }, setFromRotationMatrix: function ( m, order ) { var clamp = OIMO.clamp; // assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled) var te = m.elements; /*var m11 = te[ 0 ], m12 = te[ 4 ], m13 = te[ 8 ]; var m21 = te[ 1 ], m22 = te[ 5 ], m23 = te[ 9 ]; var m31 = te[ 2 ], m32 = te[ 6 ], m33 = te[ 10 ];*/ var m11 = te[ 0 ], m12 = te[ 1 ], m13 = te[ 2 ], m21 = te[ 3 ], m22 = te[ 4 ], m23 = te[ 5 ], m31 = te[ 6 ], m32 = te[ 7 ], m33 = te[ 8 ]; order = order || this._order; if ( order === 'XYZ' ) { this._y = Math.asin( clamp( m13, - 1, 1 ) ); if ( Math.abs( m13 ) < 0.99999 ) { this._x = Math.atan2( - m23, m33 ); this._z = Math.atan2( - m12, m11 ); } else { this._x = Math.atan2( m32, m22 ); this._z = 0; } } else if ( order === 'YXZ' ) { this._x = Math.asin( - clamp( m23, - 1, 1 ) ); if ( Math.abs( m23 ) < 0.99999 ) { this._y = Math.atan2( m13, m33 ); this._z = Math.atan2( m21, m22 ); } else { this._y = Math.atan2( - m31, m11 ); this._z = 0; } } else if ( order === 'ZXY' ) { this._x = Math.asin( clamp( m32, - 1, 1 ) ); if ( Math.abs( m32 ) < 0.99999 ) { this._y = Math.atan2( - m31, m33 ); this._z = Math.atan2( - m12, m22 ); } else { this._y = 0; this._z = Math.atan2( m21, m11 ); } } else if ( order === 'ZYX' ) { this._y = Math.asin( - clamp( m31, - 1, 1 ) ); if ( Math.abs( m31 ) < 0.99999 ) { this._x = Math.atan2( m32, m33 ); this._z = Math.atan2( m21, m11 ); } else { this._x = 0; this._z = Math.atan2( - m12, m22 ); } } else if ( order === 'YZX' ) { this._z = Math.asin( clamp( m21, - 1, 1 ) ); if ( Math.abs( m21 ) < 0.99999 ) { this._x = Math.atan2( - m23, m22 ); this._y = Math.atan2( - m31, m11 ); } else { this._x = 0; this._y = Math.atan2( m13, m33 ); } } else if ( order === 'XZY' ) { this._z = Math.asin( - clamp( m12, - 1, 1 ) ); if ( Math.abs( m12 ) < 0.99999 ) { this._x = Math.atan2( m32, m22 ); this._y = Math.atan2( m13, m11 ); } else { this._x = Math.atan2( - m23, m33 ); this._y = 0; } } else { console.warn( 'THREE.Euler: .setFromRotationMatrix() given unsupported order: ' + order ) } this._order = order; this.onChangeCallback(); return this; }, setFromQuaternion: function ( q, order, update ) { var clamp = OIMO.clamp; // q is assumed to be normalized // http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m var sqx = q.x * q.x; var sqy = q.y * q.y; var sqz = q.z * q.z; var sqw = q.s * q.s; order = order || this._order; if ( order === 'XYZ' ) { this._x = Math.atan2( 2 * ( q.x * q.s - q.y * q.z ), ( sqw - sqx - sqy + sqz ) ); this._y = Math.asin( clamp( 2 * ( q.x * q.z + q.y * q.s ), - 1, 1 ) ); this._z = Math.atan2( 2 * ( q.z * q.s - q.x * q.y ), ( sqw + sqx - sqy - sqz ) ); } else if ( order === 'YXZ' ) { this._x = Math.asin( clamp( 2 * ( q.x * q.s - q.y * q.z ), - 1, 1 ) ); this._y = Math.atan2( 2 * ( q.x * q.z + q.y * q.s ), ( sqw - sqx - sqy + sqz ) ); this._z = Math.atan2( 2 * ( q.x * q.y + q.z * q.s ), ( sqw - sqx + sqy - sqz ) ); } else if ( order === 'ZXY' ) { this._x = Math.asin( clamp( 2 * ( q.x * q.s + q.y * q.z ), - 1, 1 ) ); this._y = Math.atan2( 2 * ( q.y * q.s - q.z * q.x ), ( sqw - sqx - sqy + sqz ) ); this._z = Math.atan2( 2 * ( q.z * q.s - q.x * q.y ), ( sqw - sqx + sqy - sqz ) ); } else if ( order === 'ZYX' ) { this._x = Math.atan2( 2 * ( q.x * q.s + q.z * q.y ), ( sqw - sqx - sqy + sqz ) ); this._y = Math.asin( clamp( 2 * ( q.y * q.s - q.x * q.z ), - 1, 1 ) ); this._z = Math.atan2( 2 * ( q.x * q.y + q.z * q.s ), ( sqw + sqx - sqy - sqz ) ); } else if ( order === 'YZX' ) { this._x = Math.atan2( 2 * ( q.x * q.s - q.z * q.y ), ( sqw - sqx + sqy - sqz ) ); this._y = Math.atan2( 2 * ( q.y * q.s - q.x * q.z ), ( sqw + sqx - sqy - sqz ) ); this._z = Math.asin( clamp( 2 * ( q.x * q.y + q.z * q.s ), - 1, 1 ) ); } else if ( order === 'XZY' ) { this._x = Math.atan2( 2 * ( q.x * q.s + q.y * q.z ), ( sqw - sqx + sqy - sqz ) ); this._y = Math.atan2( 2 * ( q.x * q.z + q.y * q.s ), ( sqw + sqx - sqy - sqz ) ); this._z = Math.asin( clamp( 2 * ( q.z * q.s - q.x * q.y ), - 1, 1 ) ); } else { console.warn( 'OIMO.Euler: .setFromQuaternion() given unsupported order: ' + order ) } this._order = order; if ( update !== false ) this.onChangeCallback(); return this; }, reorder: function () { // WARNING: this discards revolution information -bhouston var q = new OIMO.Quat(); return function ( newOrder ) { q.setFromEuler( this ); this.setFromQuaternion( q, newOrder ); }; }(), equals: function ( euler ) { return ( euler._x === this._x ) && ( euler._y === this._y ) && ( euler._z === this._z ) && ( euler._order === this._order ); }, fromArray: function ( array ) { this._x = array[ 0 ]; this._y = array[ 1 ]; this._z = array[ 2 ]; if ( array[ 3 ] !== undefined ) this._order = array[ 3 ]; this.onChangeCallback(); return this; }, toArray: function () { return [ this._x, this._y, this._z, this._order ]; }, onChange: function ( callback ) { this.onChangeCallback = callback; return this; }, onChangeCallback: function () {}, clone: function () { return new OIMO.Euler( this._x, this._y, this._z, this._order ); } }; OIMO.EulerToAxis = function( ox, oy, oz ){// angles in radians var c1 = Math.cos(oy*0.5);//heading var s1 = Math.sin(oy*0.5); var c2 = Math.cos(oz*0.5);//altitude var s2 = Math.sin(oz*0.5); var c3 = Math.cos(ox*0.5);//bank var s3 = Math.sin(ox*0.5); var c1c2 = c1*c2; var s1s2 = s1*s2; var w =c1c2*c3 - s1s2*s3; var x =c1c2*s3 + s1s2*c3; var y =s1*c2*c3 + c1*s2*s3; var z =c1*s2*c3 - s1*c2*s3; var angle = 2 * Math.acos(w); var norm = x*x+y*y+z*z; if (norm < 0.001) { x=1; y=z=0; } else { norm = Math.sqrt(norm); x /= norm; y /= norm; z /= norm; } return [angle, x, y, z]; } OIMO.EulerToMatrix = function( ox, oy, oz ) {// angles in radians var ch = Math.cos(oy);//heading var sh = Math.sin(oy); var ca = Math.cos(oz);//altitude var sa = Math.sin(oz); var cb = Math.cos(ox);//bank var sb = Math.sin(ox); var mtx = new OIMO.Mat33(); var te = mtx.elements; te[0] = ch * ca; te[1] = sh*sb - ch*sa*cb; te[2] = ch*sa*sb + sh*cb; te[3] = sa; te[4] = ca*cb; te[5] = -ca*sb; te[6] = -sh*ca; te[7] = sh*sa*cb + ch*sb; te[8] = -sh*sa*sb + ch*cb; return mtx; } OIMO.MatrixToEuler = function(mtx){// angles in radians var te = mtx.elements; var x, y, z; if (te[3] > 0.998) { // singularity at north pole y = Math.atan2(te[2],te[8]); z = Math.PI/2; x = 0; } else if (te[3] < -0.998) { // singularity at south pole y = Math.atan2(te[2],te[8]); z = -Math.PI/2; x = 0; } else { y = Math.atan2(-te[6],te[0]); x = Math.atan2(-te[5],te[4]); z = Math.asin(te[3]); } return [x, y, z]; } OIMO.unwrapDegrees = function (r) { r = r % 360; if (r > 180) r -= 360; if (r < -180) r += 360; return r; } OIMO.unwrapRadian = function(r){ r = r % OIMO.TwoPI; if (r > Math.PI) r -= OIMO.TwoPI; if (r < -Math.PI) r += OIMO.TwoPI; return r; } OIMO.Distance3d = function(p1, p2){ var xd = p2[0]-p1[0]; var yd = p2[1]-p1[1]; var zd = p2[2]-p1[2]; return Math.sqrt(xd*xd + yd*yd + zd*zd); } /** * The base class of all type of the constraints. * @author saharan */ OIMO.Constraint = function(){ // The parent world of the constraint. this.parent=null; // The first body of the constraint. this.body1=null; // The second body of the constraint. this.body2=null; // Internal this.addedToIsland=false; } OIMO.Constraint.prototype = { constructor: OIMO.Constraint, /** * Prepare for solving the constraint. * @param timeStep * @param invTimeStep */ preSolve:function(timeStep,invTimeStep){ throw new Error("Inheritance error."); }, /** * Solve the constraint. * This is usually called iteratively. */ solve:function(){ throw new Error("Inheritance error."); }, /** * Do the post-processing. */ postSolve:function(){ throw new Error("Inheritance error."); } } /** * Joints are used to constrain the motion between two rigid bodies. * @author saharan */ OIMO.Joint = function(config){ OIMO.Constraint.call( this ); this.name = ""; // Distance joint this.JOINT_DISTANCE=0x1; // Ball-and-socket joint this.JOINT_BALL_AND_SOCKET=0x2; // Hinge joint this.JOINT_HINGE=0x3; // Wheel joint this.JOINT_WHEEL=0x4; // Slider joint this.JOINT_SLIDER=0x5; // Prismatic joint this.JOINT_PRISMATIC=0x6; // The type of the joint. this.type=0; // The previous joint in the world. this.prev=null; // The next joint in the world. this.next=null; this.body1=config.body1; this.body2=config.body2; // The anchor point on the first rigid body in local coordinate system. this.localAnchorPoint1=new OIMO.Vec3().copy(config.localAnchorPoint1); // The anchor point on the second rigid body in local coordinate system. this.localAnchorPoint2=new OIMO.Vec3().copy(config.localAnchorPoint2); // The anchor point on the first rigid body in world coordinate system relative to the body's origin. this.relativeAnchorPoint1=new OIMO.Vec3(); // The anchor point on the second rigid body in world coordinate system relative to the body's origin. this.relativeAnchorPoint2=new OIMO.Vec3(); // The anchor point on the first rigid body in world coordinate system. this.anchorPoint1=new OIMO.Vec3(); // The anchor point on the second rigid body in world coordinate system. this.anchorPoint2=new OIMO.Vec3(); // Whether allow collision between connected rigid bodies or not. this.allowCollision=config.allowCollision; this.b1Link=new OIMO.JointLink(this); this.b2Link=new OIMO.JointLink(this); this.matrix = new OIMO.Mat44(); } OIMO.Joint.prototype = Object.create( OIMO.Constraint.prototype ); /** * Update all the anchor points. */ OIMO.Joint.prototype.updateAnchorPoints = function () { var p1=this.body1.position; var p2=this.body2.position; var tr1 = this.body1.rotation.elements; var tr2 = this.body2.rotation.elements; var l1x=this.localAnchorPoint1.x; var l1y=this.localAnchorPoint1.y; var l1z=this.localAnchorPoint1.z; var l2x=this.localAnchorPoint2.x; var l2y=this.localAnchorPoint2.y; var l2z=this.localAnchorPoint2.z; var r1x=l1x*tr1[0]+l1y*tr1[1]+l1z*tr1[2]; var r1y=l1x*tr1[3]+l1y*tr1[4]+l1z*tr1[5]; var r1z=l1x*tr1[6]+l1y*tr1[7]+l1z*tr1[8]; var r2x=l2x*tr2[0]+l2y*tr2[1]+l2z*tr2[2]; var r2y=l2x*tr2[3]+l2y*tr2[4]+l2z*tr2[5]; var r2z=l2x*tr2[6]+l2y*tr2[7]+l2z*tr2[8]; this.relativeAnchorPoint1.x=r1x; this.relativeAnchorPoint1.y=r1y; this.relativeAnchorPoint1.z=r1z; this.relativeAnchorPoint2.x=r2x; this.relativeAnchorPoint2.y=r2y; this.relativeAnchorPoint2.z=r2z; var p1x=r1x+p1.x; var p1y=r1y+p1.y; var p1z=r1z+p1.z; var p2x=r2x+p2.x; var p2y=r2y+p2.y; var p2z=r2z+p2.z; this.anchorPoint1.x=p1x; this.anchorPoint1.y=p1y; this.anchorPoint1.z=p1z; this.anchorPoint2.x=p2x; this.anchorPoint2.y=p2y; this.anchorPoint2.z=p2z; } /** * Attach the joint from the bodies. */ OIMO.Joint.prototype.attach = function () { this.b1Link.body=this.body2; this.b2Link.body=this.body1; if(this.body1.jointLink!=null)(this.b1Link.next=this.body1.jointLink).prev=this.b1Link; else this.b1Link.next=null; this.body1.jointLink=this.b1Link; this.body1.numJoints++; if(this.body2.jointLink!=null)(this.b2Link.next=this.body2.jointLink).prev=this.b2Link; else this.b2Link.next=null; this.body2.jointLink=this.b2Link; this.body2.numJoints++; } OIMO.Joint.prototype.detach = function () { var prev=this.b1Link.prev; var next=this.b1Link.next; if(prev!=null)prev.next=next; if(next!=null)next.prev=prev; if(this.body1.jointLink==this.b1Link)this.body1.jointLink=next; this.b1Link.prev=null; this.b1Link.next=null; this.b1Link.body=null; this.body1.numJoints--; prev=this.b2Link.prev; next=this.b2Link.next; if(prev!=null)prev.next=next; if(next!=null)next.prev=prev; if(this.body2.jointLink==this.b2Link)this.body2.jointLink=next; this.b2Link.prev=null; this.b2Link.next=null; this.b2Link.body=null; this.body2.numJoints--; this.b1Link.body=null; this.b2Link.body=null; } /** * Awake the bodies. */ OIMO.Joint.prototype.awake = function () { this.body1.awake(); this.body2.awake(); } OIMO.Joint.prototype.preSolve = function (timeStep,invTimeStep) { } OIMO.Joint.prototype.solve = function () { } OIMO.Joint.prototype.postSolve = function () { } /** * Three js add */ OIMO.Joint.prototype.getPosition = function () { var p1 = new OIMO.Vec3().scale(this.anchorPoint1, OIMO.WORLD_SCALE); var p2 = new OIMO.Vec3().scale(this.anchorPoint2, OIMO.WORLD_SCALE); return [p1, p2]; } OIMO.Joint.prototype.getMatrix = function () { var m = this.matrix.elements; var p1 = this.anchorPoint1; var p2 = this.anchorPoint2; m[0] = p1.x * OIMO.WORLD_SCALE; m[1] = p1.y * OIMO.WORLD_SCALE; m[2] = p1.z * OIMO.WORLD_SCALE; m[3] = 0; m[4] = p2.x * OIMO.WORLD_SCALE; m[5] = p2.y * OIMO.WORLD_SCALE; m[6] = p2.z * OIMO.WORLD_SCALE; m[7] = 0; return m; } /** * A joint configuration holds all configuration data for constructing a joint. * Joint configurations can be reused safely. * @author saharan */ OIMO.JointConfig = function(){ // The first rigid body of the joint. this.body1 = null; // The second rigid body of the joint. this.body2 = null; // The anchor point on the first rigid body in local coordinate system. this.localAnchorPoint1=new OIMO.Vec3(); // The anchor point on the second rigid body in local coordinate system. this.localAnchorPoint2=new OIMO.Vec3(); // The axis in the first body's coordinate system. // his property is available in some joints. this.localAxis1=new OIMO.Vec3(); // The axis in the second body's coordinate system. // This property is available in some joints. this.localAxis2=new OIMO.Vec3(); // Whether allow collision between connected rigid bodies or not. this.allowCollision=false; } /** * A link list of joints. * @author saharan */ OIMO.JointLink = function(joint){ // The previous joint link. this.prev = null; // The next joint link. this.next = null; // The other rigid body connected to the joint. this.body = null; // The joint of the link. this.joint = joint; } /** * An information of limit and motor. * @author saharan */ OIMO.LimitMotor = function(axis,fixed){ // The axis of the constraint. this.axis=axis; // The current angle for rotational constraints. this.angle=0; // The lower limit. Set lower > upper to disable. if(fixed)this.lowerLimit=0; else this.lowerLimit=1; // The upper limit. Set lower > upper to disable. this.upperLimit=0; // The target motor speed. this.motorSpeed=0; // The maximum motor force or torque. Set 0 to disable. this.maxMotorForce=0; // The frequency of the spring. Set 0 to disable. this.frequency=0; // The damping ratio of the spring. Set 0 for no damping, 1 for critical damping. this.dampingRatio=0; } OIMO.LimitMotor.prototype = { constructor: OIMO.LimitMotor, /** * Set limit data into this constraint. * @param lowerLimit * @param upperLimit */ setLimit:function(lowerLimit,upperLimit){ this.lowerLimit=lowerLimit; this.upperLimit=upperLimit; }, /** * Set motor data into this constraint. * @param motorSpeed * @param maxMotorForce */ setMotor:function(motorSpeed,maxMotorForce){ this.motorSpeed=motorSpeed; this.maxMotorForce=maxMotorForce; }, /** * Set spring data into this constraint. * @param frequency * @param dampingRatio */ setSpring:function(frequency,dampingRatio){ this.frequency=frequency; this.dampingRatio=dampingRatio; } } /** * A ball-and-socket joint limits relative translation on two anchor points on rigid bodies. * @author saharan */ OIMO.BallAndSocketJoint = function(config){ OIMO.Joint.call( this, config); this.type=this.JOINT_BALL_AND_SOCKET; this.lc=new OIMO.LinearConstraint(this); } OIMO.BallAndSocketJoint.prototype = Object.create( OIMO.Joint.prototype ); OIMO.BallAndSocketJoint.prototype.preSolve = function (timeStep,invTimeStep) { this.updateAnchorPoints(); this.lc.preSolve(timeStep,invTimeStep); } OIMO.BallAndSocketJoint.prototype.solve = function () { this.lc.solve(); } OIMO.BallAndSocketJoint.prototype.postSolve = function () { } /** * A distance joint limits the distance between two anchor points on rigid bodies. * @author saharan */ OIMO.DistanceJoint = function(config,minDistance,maxDistance){ OIMO.Joint.call( this, config); this.type=this.JOINT_DISTANCE; this.normal=new OIMO.Vec3(); // The limit and motor information of the joint. this.limitMotor=new OIMO.LimitMotor(this.normal,true); this.limitMotor.lowerLimit=minDistance; this.limitMotor.upperLimit=maxDistance; this.t=new OIMO.TranslationalConstraint(this,this.limitMotor); } OIMO.DistanceJoint.prototype = Object.create( OIMO.Joint.prototype ); OIMO.DistanceJoint.prototype.preSolve = function (timeStep,invTimeStep) { this.updateAnchorPoints(); var nx=this.anchorPoint2.x-this.anchorPoint1.x; var ny=this.anchorPoint2.y-this.anchorPoint1.y; var nz=this.anchorPoint2.z-this.anchorPoint1.z; var len=Math.sqrt(nx*nx+ny*ny+nz*nz); if(len>0)len=1/len; this.normal.init(nx*len,ny*len,nz*len); this.t.preSolve(timeStep,invTimeStep); } OIMO.DistanceJoint.prototype.solve = function () { this.t.solve(); } OIMO.DistanceJoint.prototype.postSolve = function () { } /** * A hinge joint allows only for relative rotation of rigid bodies along the axis. * @author saharan */ OIMO.HingeJoint = function(config,lowerAngleLimit,upperAngleLimit){ OIMO.Joint.call( this, config); this.type=this.JOINT_HINGE; // The axis in the first body's coordinate system. this.localAxis1=new OIMO.Vec3().normalize(config.localAxis1); // The axis in the second body's coordinate system. this.localAxis2=new OIMO.Vec3().normalize(config.localAxis2); var len; this.localAxis1X=this.localAxis1.x; this.localAxis1Y=this.localAxis1.y; this.localAxis1Z=this.localAxis1.z; this.localAngAxis1X=this.localAxis1Y*this.localAxis1X-this.localAxis1Z*this.localAxis1Z; this.localAngAxis1Y=-this.localAxis1Z*this.localAxis1Y-this.localAxis1X*this.localAxis1X; this.localAngAxis1Z=this.localAxis1X*this.localAxis1Z+this.localAxis1Y*this.localAxis1Y; len=1/Math.sqrt(this.localAngAxis1X*this.localAngAxis1X+this.localAngAxis1Y*this.localAngAxis1Y+this.localAngAxis1Z*this.localAngAxis1Z); this.localAngAxis1X*=len; this.localAngAxis1Y*=len; this.localAngAxis1Z*=len; this.localAxis2X=this.localAxis2.x; this.localAxis2Y=this.localAxis2.y; this.localAxis2Z=this.localAxis2.z; // make angle axis 2 var arc=new OIMO.Mat33().setQuat(new OIMO.Quat().arc(this.localAxis1,this.localAxis2)); var tarc = arc.elements; this.localAngAxis2X=this.localAngAxis1X*tarc[0]+this.localAngAxis1Y*tarc[1]+this.localAngAxis1Z*tarc[2]; this.localAngAxis2Y=this.localAngAxis1X*tarc[3]+this.localAngAxis1Y*tarc[4]+this.localAngAxis1Z*tarc[5]; this.localAngAxis2Z=this.localAngAxis1X*tarc[6]+this.localAngAxis1Y*tarc[7]+this.localAngAxis1Z*tarc[8]; this.nor=new OIMO.Vec3(); this.tan=new OIMO.Vec3(); this.bin=new OIMO.Vec3(); // The rotational limit and motor information of the joint. this.limitMotor=new OIMO.LimitMotor(this.nor,false); this.limitMotor.lowerLimit=lowerAngleLimit; this.limitMotor.upperLimit=upperAngleLimit; this.lc=new OIMO.LinearConstraint(this); this.r3=new OIMO.Rotational3Constraint(this,this.limitMotor,new OIMO.LimitMotor(this.tan,true),new OIMO.LimitMotor(this.bin,true)); } OIMO.HingeJoint.prototype = Object.create( OIMO.Joint.prototype ); OIMO.HingeJoint.prototype.preSolve = function (timeStep,invTimeStep) { var tmpM; var tmp1X; var tmp1Y; var tmp1Z; this.updateAnchorPoints(); tmpM=this.body1.rotation.elements; var axis1X=this.localAxis1X*tmpM[0]+this.localAxis1Y*tmpM[1]+this.localAxis1Z*tmpM[2]; var axis1Y=this.localAxis1X*tmpM[3]+this.localAxis1Y*tmpM[4]+this.localAxis1Z*tmpM[5]; var axis1Z=this.localAxis1X*tmpM[6]+this.localAxis1Y*tmpM[7]+this.localAxis1Z*tmpM[8]; var angAxis1X=this.localAngAxis1X*tmpM[0]+this.localAngAxis1Y*tmpM[1]+this.localAngAxis1Z*tmpM[2]; var angAxis1Y=this.localAngAxis1X*tmpM[3]+this.localAngAxis1Y*tmpM[4]+this.localAngAxis1Z*tmpM[5]; var angAxis1Z=this.localAngAxis1X*tmpM[6]+this.localAngAxis1Y*tmpM[7]+this.localAngAxis1Z*tmpM[8]; tmpM=this.body2.rotation.elements; var axis2X=this.localAxis2X*tmpM[0]+this.localAxis2Y*tmpM[1]+this.localAxis2Z*tmpM[2]; var axis2Y=this.localAxis2X*tmpM[3]+this.localAxis2Y*tmpM[4]+this.localAxis2Z*tmpM[5]; var axis2Z=this.localAxis2X*tmpM[6]+this.localAxis2Y*tmpM[7]+this.localAxis2Z*tmpM[8]; var angAxis2X=this.localAngAxis2X*tmpM[0]+this.localAngAxis2Y*tmpM[1]+this.localAngAxis2Z*tmpM[2]; var angAxis2Y=this.localAngAxis2X*tmpM[3]+this.localAngAxis2Y*tmpM[4]+this.localAngAxis2Z*tmpM[5]; var angAxis2Z=this.localAngAxis2X*tmpM[6]+this.localAngAxis2Y*tmpM[7]+this.localAngAxis2Z*tmpM[8]; var nx=axis1X*this.body2.inverseMass+axis2X*this.body1.inverseMass; var ny=axis1Y*this.body2.inverseMass+axis2Y*this.body1.inverseMass; var nz=axis1Z*this.body2.inverseMass+axis2Z*this.body1.inverseMass; tmp1X=Math.sqrt(nx*nx+ny*ny+nz*nz); if(tmp1X>0)tmp1X=1/tmp1X; nx*=tmp1X; ny*=tmp1X; nz*=tmp1X; var tx=ny*nx-nz*nz; var ty=-nz*ny-nx*nx; var tz=nx*nz+ny*ny; tmp1X=1/Math.sqrt(tx*tx+ty*ty+tz*tz); tx*=tmp1X; ty*=tmp1X; tz*=tmp1X; var bx=ny*tz-nz*ty; var by=nz*tx-nx*tz; var bz=nx*ty-ny*tx; this.nor.init(nx,ny,nz); this.tan.init(tx,ty,tz); this.bin.init(bx,by,bz); // ---------------------------------------------- // calculate hinge angle // ---------------------------------------------- if( nx*(angAxis1Y*angAxis2Z-angAxis1Z*angAxis2Y)+ ny*(angAxis1Z*angAxis2X-angAxis1X*angAxis2Z)+ nz*(angAxis1X*angAxis2Y-angAxis1Y*angAxis2X)<0 ){ this.limitMotor.angle=-this.acosClamp(angAxis1X*angAxis2X+angAxis1Y*angAxis2Y+angAxis1Z*angAxis2Z); }else{ this.limitMotor.angle=this.acosClamp(angAxis1X*angAxis2X+angAxis1Y*angAxis2Y+angAxis1Z*angAxis2Z); } // angular error tmp1X=axis1Y*axis2Z-axis1Z*axis2Y; tmp1Y=axis1Z*axis2X-axis1X*axis2Z; tmp1Z=axis1X*axis2Y-axis1Y*axis2X; this.r3.limitMotor2.angle=tx*tmp1X+ty*tmp1Y+tz*tmp1Z; this.r3.limitMotor3.angle=bx*tmp1X+by*tmp1Y+bz*tmp1Z; this.r3.preSolve(timeStep,invTimeStep); this.lc.preSolve(timeStep,invTimeStep); } OIMO.HingeJoint.prototype.solve = function () { this.r3.solve(); this.lc.solve(); } OIMO.HingeJoint.prototype.postSolve = function () { } OIMO.HingeJoint.prototype.acosClamp = function(cos){ if(cos>1)return 0; else if(cos<-1)return Math.PI; else return Math.acos(cos); } /** * A prismatic joint allows only for relative translation of rigid bodies along the axis. * @author saharan */ OIMO.PrismaticJoint = function(config,lowerTranslation,upperTranslation){ OIMO.Joint.call( this, config); this.type=this.JOINT_PRISMATIC; // The axis in the first body's coordinate system. this.localAxis1=new OIMO.Vec3().normalize(config.localAxis1); // The axis in the second body's coordinate system. this.localAxis2=new OIMO.Vec3().normalize(config.localAxis2); this.localAxis1X=this.localAxis1.x; this.localAxis1Y=this.localAxis1.y; this.localAxis1Z=this.localAxis1.z; this.localAxis2X=this.localAxis2.x; this.localAxis2Y=this.localAxis2.y; this.localAxis2Z=this.localAxis2.z; this.nor=new OIMO.Vec3(); this.tan=new OIMO.Vec3(); this.bin=new OIMO.Vec3(); this.ac=new OIMO.AngularConstraint(this,new OIMO.Quat().arc(this.localAxis1,this.localAxis2)); // The translational limit and motor information of the joint. this.limitMotor=new OIMO.LimitMotor(this.nor,true); this.limitMotor.lowerLimit=lowerTranslation; this.limitMotor.upperLimit=upperTranslation; this.t3=new OIMO.Translational3Constraint(this,this.limitMotor,new OIMO.LimitMotor(this.tan,true),new OIMO.LimitMotor(this.bin,true)); } OIMO.PrismaticJoint.prototype = Object.create( OIMO.Joint.prototype ); OIMO.PrismaticJoint.prototype.preSolve = function (timeStep,invTimeStep) { var tmpM; var tmp1X; var tmp1Y; var tmp1Z; this.updateAnchorPoints(); tmpM=this.body1.rotation.elements; var axis1X=this.localAxis1X*tmpM[0]+this.localAxis1Y*tmpM[1]+this.localAxis1Z*tmpM[2]; var axis1Y=this.localAxis1X*tmpM[3]+this.localAxis1Y*tmpM[4]+this.localAxis1Z*tmpM[5]; var axis1Z=this.localAxis1X*tmpM[6]+this.localAxis1Y*tmpM[7]+this.localAxis1Z*tmpM[8]; tmpM=this.body2.rotation.elements; var axis2X=this.localAxis2X*tmpM[0]+this.localAxis2Y*tmpM[1]+this.localAxis2Z*tmpM[2]; var axis2Y=this.localAxis2X*tmpM[3]+this.localAxis2Y*tmpM[4]+this.localAxis2Z*tmpM[5]; var axis2Z=this.localAxis2X*tmpM[6]+this.localAxis2Y*tmpM[7]+this.localAxis2Z*tmpM[8]; var nx=axis1X*this.body2.inverseMass+axis2X*this.body1.inverseMass; var ny=axis1Y*this.body2.inverseMass+axis2Y*this.body1.inverseMass; var nz=axis1Z*this.body2.inverseMass+axis2Z*this.body1.inverseMass; tmp1X=Math.sqrt(nx*nx+ny*ny+nz*nz); if(tmp1X>0)tmp1X=1/tmp1X; nx*=tmp1X; ny*=tmp1X; nz*=tmp1X; var tx=ny*nx-nz*nz; var ty=-nz*ny-nx*nx; var tz=nx*nz+ny*ny; tmp1X=1/Math.sqrt(tx*tx+ty*ty+tz*tz); tx*=tmp1X; ty*=tmp1X; tz*=tmp1X; var bx=ny*tz-nz*ty; var by=nz*tx-nx*tz; var bz=nx*ty-ny*tx; this.nor.init(nx,ny,nz); this.tan.init(tx,ty,tz); this.bin.init(bx,by,bz); this.ac.preSolve(timeStep,invTimeStep); this.t3.preSolve(timeStep,invTimeStep); } OIMO.PrismaticJoint.prototype.solve = function () { this.ac.solve(); this.t3.solve(); } OIMO.PrismaticJoint.prototype.postSolve = function () { } /** * A slider joint allows for relative translation and relative rotation between two rigid bodies along the axis. * @author saharan */ OIMO.SliderJoint = function(config,lowerTranslation,upperTranslation){ OIMO.Joint.call( this, config); this.type=this.JOINT_SLIDER; // The first axis in local coordinate system. this.localAxis1=new OIMO.Vec3().normalize(config.localAxis1); // The second axis in local coordinate system. this.localAxis2=new OIMO.Vec3().normalize(config.localAxis2); var len; this.localAxis1X=this.localAxis1.x; this.localAxis1Y=this.localAxis1.y; this.localAxis1Z=this.localAxis1.z; this.localAngAxis1X=this.localAxis1Y*this.localAxis1X-this.localAxis1Z*this.localAxis1Z; this.localAngAxis1Y=-this.localAxis1Z*this.localAxis1Y-this.localAxis1X*this.localAxis1X; this.localAngAxis1Z=this.localAxis1X*this.localAxis1Z+this.localAxis1Y*this.localAxis1Y; len=1/Math.sqrt(this.localAngAxis1X*this.localAngAxis1X+this.localAngAxis1Y*this.localAngAxis1Y+this.localAngAxis1Z*this.localAngAxis1Z); this.localAngAxis1X*=len; this.localAngAxis1Y*=len; this.localAngAxis1Z*=len; this.localAxis2X=this.localAxis2.x; this.localAxis2Y=this.localAxis2.y; this.localAxis2Z=this.localAxis2.z; // make angle axis 2 var arc=new OIMO.Mat33().setQuat(new OIMO.Quat().arc(this.localAxis1,this.localAxis2)); var tarc = arc.elements; this.localAngAxis2X=this.localAngAxis1X*tarc[0]+this.localAngAxis1Y*tarc[1]+this.localAngAxis1Z*tarc[2]; this.localAngAxis2Y=this.localAngAxis1X*tarc[3]+this.localAngAxis1Y*tarc[4]+this.localAngAxis1Z*tarc[5]; this.localAngAxis2Z=this.localAngAxis1X*tarc[6]+this.localAngAxis1Y*tarc[7]+this.localAngAxis1Z*tarc[8]; this.nor=new OIMO.Vec3(); this.tan=new OIMO.Vec3(); this.bin=new OIMO.Vec3(); // The limit and motor for the rotation this.rotationalLimitMotor=new OIMO.LimitMotor(this.nor,false); this.r3=new OIMO.Rotational3Constraint(this,this.rotationalLimitMotor,new OIMO.LimitMotor(this.tan,true),new OIMO.LimitMotor(this.bin,true)); // The limit and motor for the translation. this.translationalLimitMotor=new OIMO.LimitMotor(this.nor,true); this.translationalLimitMotor.lowerLimit=lowerTranslation; this.translationalLimitMotor.upperLimit=upperTranslation; this.t3=new OIMO.Translational3Constraint(this,this.translationalLimitMotor,new OIMO.LimitMotor(this.tan,true),new OIMO.LimitMotor(this.bin,true)); } OIMO.SliderJoint.prototype = Object.create( OIMO.Joint.prototype ); OIMO.SliderJoint.prototype.preSolve = function (timeStep,invTimeStep) { var tmpM; var tmp1X; var tmp1Y; var tmp1Z; this.updateAnchorPoints(); tmpM=this.body1.rotation.elements; var axis1X=this.localAxis1X*tmpM[0]+this.localAxis1Y*tmpM[1]+this.localAxis1Z*tmpM[2]; var axis1Y=this.localAxis1X*tmpM[3]+this.localAxis1Y*tmpM[4]+this.localAxis1Z*tmpM[5]; var axis1Z=this.localAxis1X*tmpM[6]+this.localAxis1Y*tmpM[7]+this.localAxis1Z*tmpM[8]; var angAxis1X=this.localAngAxis1X*tmpM[0]+this.localAngAxis1Y*tmpM[1]+this.localAngAxis1Z*tmpM[2]; var angAxis1Y=this.localAngAxis1X*tmpM[3]+this.localAngAxis1Y*tmpM[4]+this.localAngAxis1Z*tmpM[5]; var angAxis1Z=this.localAngAxis1X*tmpM[6]+this.localAngAxis1Y*tmpM[7]+this.localAngAxis1Z*tmpM[8]; tmpM=this.body2.rotation.elements; var axis2X=this.localAxis2X*tmpM[0]+this.localAxis2Y*tmpM[1]+this.localAxis2Z*tmpM[2]; var axis2Y=this.localAxis2X*tmpM[3]+this.localAxis2Y*tmpM[4]+this.localAxis2Z*tmpM[5]; var axis2Z=this.localAxis2X*tmpM[6]+this.localAxis2Y*tmpM[7]+this.localAxis2Z*tmpM[8]; var angAxis2X=this.localAngAxis2X*tmpM[0]+this.localAngAxis2Y*tmpM[1]+this.localAngAxis2Z*tmpM[2]; var angAxis2Y=this.localAngAxis2X*tmpM[3]+this.localAngAxis2Y*tmpM[4]+this.localAngAxis2Z*tmpM[5]; var angAxis2Z=this.localAngAxis2X*tmpM[6]+this.localAngAxis2Y*tmpM[7]+this.localAngAxis2Z*tmpM[8]; var nx=axis1X*this.body2.inverseMass+axis2X*this.body1.inverseMass; var ny=axis1Y*this.body2.inverseMass+axis2Y*this.body1.inverseMass; var nz=axis1Z*this.body2.inverseMass+axis2Z*this.body1.inverseMass; tmp1X=Math.sqrt(nx*nx+ny*ny+nz*nz); if(tmp1X>0)tmp1X=1/tmp1X; nx*=tmp1X; ny*=tmp1X; nz*=tmp1X; var tx=ny*nx-nz*nz; var ty=-nz*ny-nx*nx; var tz=nx*nz+ny*ny; tmp1X=1/Math.sqrt(tx*tx+ty*ty+tz*tz); tx*=tmp1X; ty*=tmp1X; tz*=tmp1X; var bx=ny*tz-nz*ty; var by=nz*tx-nx*tz; var bz=nx*ty-ny*tx; this.nor.init(nx,ny,nz); this.tan.init(tx,ty,tz); this.bin.init(bx,by,bz); // ---------------------------------------------- // calculate hinge angle // ---------------------------------------------- if( nx*(angAxis1Y*angAxis2Z-angAxis1Z*angAxis2Y)+ ny*(angAxis1Z*angAxis2X-angAxis1X*angAxis2Z)+ nz*(angAxis1X*angAxis2Y-angAxis1Y*angAxis2X)<0 ){ this.rotationalLimitMotor.angle=-this.acosClamp(angAxis1X*angAxis2X+angAxis1Y*angAxis2Y+angAxis1Z*angAxis2Z); }else{ this.rotationalLimitMotor.angle=this.acosClamp(angAxis1X*angAxis2X+angAxis1Y*angAxis2Y+angAxis1Z*angAxis2Z); } // angular error tmp1X=axis1Y*axis2Z-axis1Z*axis2Y; tmp1Y=axis1Z*axis2X-axis1X*axis2Z; tmp1Z=axis1X*axis2Y-axis1Y*axis2X; this.r3.limitMotor2.angle=tx*tmp1X+ty*tmp1Y+tz*tmp1Z; this.r3.limitMotor3.angle=bx*tmp1X+by*tmp1Y+bz*tmp1Z; this.r3.preSolve(timeStep,invTimeStep); this.t3.preSolve(timeStep,invTimeStep); } OIMO.SliderJoint.prototype.solve = function () { this.r3.solve(); this.t3.solve(); } OIMO.SliderJoint.prototype.postSolve = function () { } OIMO.SliderJoint.prototype.acosClamp = function(cos){ if(cos>1)return 0; else if(cos<-1)return Math.PI; else return Math.acos(cos); } /** * A wheel joint allows for relative rotation between two rigid bodies along two axes. * The wheel joint also allows for relative translation for the suspension. */ OIMO.WheelJoint = function(config){ OIMO.Joint.call( this, config); this.type=this.JOINT_WHEEL; // The first axis in local coordinate system. this.localAxis1=new OIMO.Vec3().normalize(config.localAxis1); // The second axis in local coordinate system. this.localAxis2=new OIMO.Vec3().normalize(config.localAxis2); var len; this.localAxis1X=this.localAxis1.x; this.localAxis1Y=this.localAxis1.y; this.localAxis1Z=this.localAxis1.z; this.localAxis2X=this.localAxis2.x; this.localAxis2Y=this.localAxis2.y; this.localAxis2Z=this.localAxis2.z; var dot=this.localAxis1X*this.localAxis2X+this.localAxis1Y*this.localAxis2Y+this.localAxis1Z*this.localAxis2Z; if(dot>-1&&dot<1){ this.localAngAxis1X=this.localAxis2X-dot*this.localAxis1X; this.localAngAxis1Y=this.localAxis2Y-dot*this.localAxis1Y; this.localAngAxis1Z=this.localAxis2Z-dot*this.localAxis1Z; this.localAngAxis2X=this.localAxis1X-dot*this.localAxis2X; this.localAngAxis2Y=this.localAxis1Y-dot*this.localAxis2Y; this.localAngAxis2Z=this.localAxis1Z-dot*this.localAxis2Z; len=1/Math.sqrt(this.localAngAxis1X*this.localAngAxis1X+this.localAngAxis1Y*this.localAngAxis1Y+this.localAngAxis1Z*this.localAngAxis1Z); this.localAngAxis1X*=len; this.localAngAxis1Y*=len; this.localAngAxis1Z*=len; len=1/Math.sqrt(this.localAngAxis2X*this.localAngAxis2X+this.localAngAxis2Y*this.localAngAxis2Y+this.localAngAxis2Z*this.localAngAxis2Z); this.localAngAxis2X*=len; this.localAngAxis2Y*=len; this.localAngAxis2Z*=len; }else{ this.localAngAxis1X=this.localAxis1Y*this.localAxis1X-this.localAxis1Z*this.localAxis1Z; this.localAngAxis1Y=-this.localAxis1Z*this.localAxis1Y-this.localAxis1X*this.localAxis1X; this.localAngAxis1Z=this.localAxis1X*this.localAxis1Z+this.localAxis1Y*this.localAxis1Y; len=1/Math.sqrt(this.localAngAxis1X*this.localAngAxis1X+this.localAngAxis1Y*this.localAngAxis1Y+this.localAngAxis1Z*this.localAngAxis1Z); this.localAngAxis1X*=len; this.localAngAxis1Y*=len; this.localAngAxis1Z*=len; var arc=new OIMO.Mat33().setQuat(new OIMO.Quat().arc(this.localAxis1,this.localAxis2)); var tarc = arc.elements; this.localAngAxis2X=this.localAngAxis1X*tarc[0]+this.localAngAxis1Y*tarc[1]+this.localAngAxis1Z*tarc[2]; this.localAngAxis2Y=this.localAngAxis1X*tarc[3]+this.localAngAxis1Y*tarc[4]+this.localAngAxis1Z*tarc[5]; this.localAngAxis2Z=this.localAngAxis1X*tarc[6]+this.localAngAxis1Y*tarc[7]+this.localAngAxis1Z*tarc[8]; } this.nor=new OIMO.Vec3(); this.tan=new OIMO.Vec3(); this.bin=new OIMO.Vec3(); // The translational limit and motor information of the joint. this.translationalLimitMotor=new OIMO.LimitMotor(this.tan,true); this.translationalLimitMotor.frequency=8; this.translationalLimitMotor.dampingRatio=1; // The first rotational limit and motor information of the joint. this.rotationalLimitMotor1=new OIMO.LimitMotor(this.tan,false); // The second rotational limit and motor information of the joint. this.rotationalLimitMotor2=new OIMO.LimitMotor(this.bin,false); this.t3=new OIMO.Translational3Constraint(this,new OIMO.LimitMotor(this.nor,true),this.translationalLimitMotor,new OIMO.LimitMotor(this.bin,true)); this.t3.weight=1; this.r3=new OIMO.Rotational3Constraint(this,new OIMO.LimitMotor(this.nor,true),this.rotationalLimitMotor1,this.rotationalLimitMotor2); } OIMO.WheelJoint.prototype = Object.create( OIMO.Joint.prototype ); OIMO.WheelJoint.prototype.preSolve = function (timeStep,invTimeStep) { var tmpM; var tmp1X; var tmp1Y; var tmp1Z; this.updateAnchorPoints(); tmpM=this.body1.rotation.elements; var x1=this.localAxis1X*tmpM[0]+this.localAxis1Y*tmpM[1]+this.localAxis1Z*tmpM[2]; var y1=this.localAxis1X*tmpM[3]+this.localAxis1Y*tmpM[4]+this.localAxis1Z*tmpM[5]; var z1=this.localAxis1X*tmpM[6]+this.localAxis1Y*tmpM[7]+this.localAxis1Z*tmpM[8]; var angAxis1X=this.localAngAxis1X*tmpM[0]+this.localAngAxis1Y*tmpM[1]+this.localAngAxis1Z*tmpM[2]; var angAxis1Y=this.localAngAxis1X*tmpM[3]+this.localAngAxis1Y*tmpM[4]+this.localAngAxis1Z*tmpM[5]; var angAxis1Z=this.localAngAxis1X*tmpM[6]+this.localAngAxis1Y*tmpM[7]+this.localAngAxis1Z*tmpM[8]; tmpM=this.body2.rotation.elements; var x2=this.localAxis2X*tmpM[0]+this.localAxis2Y*tmpM[1]+this.localAxis2Z*tmpM[2]; var y2=this.localAxis2X*tmpM[3]+this.localAxis2Y*tmpM[4]+this.localAxis2Z*tmpM[5]; var z2=this.localAxis2X*tmpM[6]+this.localAxis2Y*tmpM[7]+this.localAxis2Z*tmpM[8]; var angAxis2X=this.localAngAxis2X*tmpM[0]+this.localAngAxis2Y*tmpM[1]+this.localAngAxis2Z*tmpM[2]; var angAxis2Y=this.localAngAxis2X*tmpM[3]+this.localAngAxis2Y*tmpM[4]+this.localAngAxis2Z*tmpM[5]; var angAxis2Z=this.localAngAxis2X*tmpM[6]+this.localAngAxis2Y*tmpM[7]+this.localAngAxis2Z*tmpM[8]; this.r3.limitMotor1.angle=x1*x2+y1*y2+z1*z2; if( x1*(angAxis1Y*z2-angAxis1Z*y2)+ y1*(angAxis1Z*x2-angAxis1X*z2)+ z1*(angAxis1X*y2-angAxis1Y*x2)<0 ){ this.rotationalLimitMotor1.angle=-this.acosClamp(angAxis1X*x2+angAxis1Y*y2+angAxis1Z*z2); }else{ this.rotationalLimitMotor1.angle=this.acosClamp(angAxis1X*x2+angAxis1Y*y2+angAxis1Z*z2); } if( x2*(angAxis2Y*z1-angAxis2Z*y1)+ y2*(angAxis2Z*x1-angAxis2X*z1)+ z2*(angAxis2X*y1-angAxis2Y*x1)<0 ){ this.rotationalLimitMotor2.angle=this.acosClamp(angAxis2X*x1+angAxis2Y*y1+angAxis2Z*z1); }else{ this.rotationalLimitMotor2.angle=-this.acosClamp(angAxis2X*x1+angAxis2Y*y1+angAxis2Z*z1); } var nx=y2*z1-z2*y1; var ny=z2*x1-x2*z1; var nz=x2*y1-y2*x1; tmp1X=Math.sqrt(nx*nx+ny*ny+nz*nz); if(tmp1X>0)tmp1X=1/tmp1X; nx*=tmp1X; ny*=tmp1X; nz*=tmp1X; var tx=ny*z2-nz*y2; var ty=nz*x2-nx*z2; var tz=nx*y2-ny*x2; tmp1X=Math.sqrt(tx*tx+ty*ty+tz*tz); if(tmp1X>0)tmp1X=1/tmp1X; tx*=tmp1X; ty*=tmp1X; tz*=tmp1X; var bx=y1*nz-z1*ny; var by=z1*nx-x1*nz; var bz=x1*ny-y1*nx; tmp1X=Math.sqrt(bx*bx+by*by+bz*bz); if(tmp1X>0)tmp1X=1/tmp1X; bx*=tmp1X; by*=tmp1X; bz*=tmp1X; this.nor.init(nx,ny,nz); this.tan.init(tx,ty,tz); this.bin.init(bx,by,bz); this.r3.preSolve(timeStep,invTimeStep); this.t3.preSolve(timeStep,invTimeStep); } OIMO.WheelJoint.prototype.solve = function () { this.r3.solve(); this.t3.solve(); } OIMO.WheelJoint.prototype.postSolve = function () { } OIMO.WheelJoint.prototype.acosClamp = function(cos){ if(cos>1)return 0; else if(cos<-1)return Math.PI; else return Math.acos(cos); } /** * An angular constraint for all axes for various joints. * @author saharan */ OIMO.AngularConstraint = function(joint,targetOrientation){ this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.d00=NaN; this.d01=NaN; this.d02=NaN; this.d10=NaN; this.d11=NaN; this.d12=NaN; this.d20=NaN; this.d21=NaN; this.d22=NaN; this.ax=NaN; this.ay=NaN; this.az=NaN; this.velx=NaN; this.vely=NaN; this.velz=NaN; this.joint=joint; this.targetOrientation=new OIMO.Quat().invert(targetOrientation); this.relativeOrientation=new OIMO.Quat(); this.b1=joint.body1; this.b2=joint.body2; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.impx=0; this.impy=0; this.impz=0; } OIMO.AngularConstraint.prototype = { constructor: OIMO.AngularConstraint, preSolve:function(timeStep,invTimeStep){ var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; var v00=this.i1e00+this.i2e00; var v01=this.i1e01+this.i2e01; var v02=this.i1e02+this.i2e02; var v10=this.i1e10+this.i2e10; var v11=this.i1e11+this.i2e11; var v12=this.i1e12+this.i2e12; var v20=this.i1e20+this.i2e20; var v21=this.i1e21+this.i2e21; var v22=this.i1e22+this.i2e22; var inv=1/( v00*(v11*v22-v21*v12)+ v10*(v21*v02-v01*v22)+ v20*(v01*v12-v11*v02) ); this.d00=(v11*v22-v12*v21)*inv; this.d01=(v02*v21-v01*v22)*inv; this.d02=(v01*v12-v02*v11)*inv; this.d10=(v12*v20-v10*v22)*inv; this.d11=(v00*v22-v02*v20)*inv; this.d12=(v02*v10-v00*v12)*inv; this.d20=(v10*v21-v11*v20)*inv; this.d21=(v01*v20-v00*v21)*inv; this.d22=(v00*v11-v01*v10)*inv; this.relativeOrientation.invert(this.b1.orientation);// error = b2 - b1 - target this.relativeOrientation.mul(this.targetOrientation,this.relativeOrientation); this.relativeOrientation.mul(this.b2.orientation,this.relativeOrientation); inv=this.relativeOrientation.s*2; this.velx=this.relativeOrientation.x*inv; this.vely=this.relativeOrientation.y*inv; this.velz=this.relativeOrientation.z*inv; var len=Math.sqrt(this.velx*this.velx+this.vely*this.vely+this.velz*this.velz); if(len>0.02){ len=(0.02-len)/len*invTimeStep*0.05; this.velx*=len; this.vely*=len; this.velz*=len; }else{ this.velx=0; this.vely=0; this.velz=0; } this.a1.x+=this.impx*this.i1e00+this.impy*this.i1e01+this.impz*this.i1e02; this.a1.y+=this.impx*this.i1e10+this.impy*this.i1e11+this.impz*this.i1e12; this.a1.z+=this.impx*this.i1e20+this.impy*this.i1e21+this.impz*this.i1e22; this.a2.x-=this.impx*this.i2e00+this.impy*this.i2e01+this.impz*this.i2e02; this.a2.y-=this.impx*this.i2e10+this.impy*this.i2e11+this.impz*this.i2e12; this.a2.z-=this.impx*this.i2e20+this.impy*this.i2e21+this.impz*this.i2e22; }, solve:function(){ var rvx=this.a2.x-this.a1.x-this.velx; var rvy=this.a2.y-this.a1.y-this.vely; var rvz=this.a2.z-this.a1.z-this.velz; var nimpx=rvx*this.d00+rvy*this.d01+rvz*this.d02; var nimpy=rvx*this.d10+rvy*this.d11+rvz*this.d12; var nimpz=rvx*this.d20+rvy*this.d21+rvz*this.d22; this.impx+=nimpx; this.impy+=nimpy; this.impz+=nimpz; this.a1.x+=nimpx*this.i1e00+nimpy*this.i1e01+nimpz*this.i1e02; this.a1.y+=nimpx*this.i1e10+nimpy*this.i1e11+nimpz*this.i1e12; this.a1.z+=nimpx*this.i1e20+nimpy*this.i1e21+nimpz*this.i1e22; this.a2.x-=nimpx*this.i2e00+nimpy*this.i2e01+nimpz*this.i2e02; this.a2.y-=nimpx*this.i2e10+nimpy*this.i2e11+nimpz*this.i2e12; this.a2.z-=nimpx*this.i2e20+nimpy*this.i2e21+nimpz*this.i2e22; } } /** * A linear constraint for all axes for various joints. * @author saharan */ OIMO.LinearConstraint = function(joint){ this.m1=NaN; this.m2=NaN; this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.d00=NaN; this.d01=NaN; this.d02=NaN; this.d10=NaN; this.d11=NaN; this.d12=NaN; this.d20=NaN; this.d21=NaN; this.d22=NaN; this.r1x=NaN; this.r1y=NaN; this.r1z=NaN; this.r2x=NaN; this.r2y=NaN; this.r2z=NaN; this.ax1x=NaN; this.ax1y=NaN; this.ax1z=NaN; this.ay1x=NaN; this.ay1y=NaN; this.ay1z=NaN; this.az1x=NaN; this.az1y=NaN; this.az1z=NaN; this.ax2x=NaN; this.ax2y=NaN; this.ax2z=NaN; this.ay2x=NaN; this.ay2y=NaN; this.ay2z=NaN; this.az2x=NaN; this.az2y=NaN; this.az2z=NaN; this.vel=NaN; this.velx=NaN; this.vely=NaN; this.velz=NaN; this.joint=joint; this.r1=joint.relativeAnchorPoint1; this.r2=joint.relativeAnchorPoint2; this.p1=joint.anchorPoint1; this.p2=joint.anchorPoint2; this.b1=joint.body1; this.b2=joint.body2; this.l1=this.b1.linearVelocity; this.l2=this.b2.linearVelocity; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.impx=0; this.impy=0; this.impz=0; } OIMO.LinearConstraint.prototype = { constructor: OIMO.LinearConstraint, preSolve:function(timeStep,invTimeStep){ this.r1x=this.r1.x; this.r1y=this.r1.y; this.r1z=this.r1.z; this.r2x=this.r2.x; this.r2y=this.r2.y; this.r2z=this.r2.z; this.m1=this.b1.inverseMass; this.m2=this.b2.inverseMass; var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; this.ax1x=this.r1z*this.i1e01+-this.r1y*this.i1e02; this.ax1y=this.r1z*this.i1e11+-this.r1y*this.i1e12; this.ax1z=this.r1z*this.i1e21+-this.r1y*this.i1e22; this.ay1x=-this.r1z*this.i1e00+this.r1x*this.i1e02; this.ay1y=-this.r1z*this.i1e10+this.r1x*this.i1e12; this.ay1z=-this.r1z*this.i1e20+this.r1x*this.i1e22; this.az1x=this.r1y*this.i1e00+-this.r1x*this.i1e01; this.az1y=this.r1y*this.i1e10+-this.r1x*this.i1e11; this.az1z=this.r1y*this.i1e20+-this.r1x*this.i1e21; this.ax2x=this.r2z*this.i2e01+-this.r2y*this.i2e02; this.ax2y=this.r2z*this.i2e11+-this.r2y*this.i2e12; this.ax2z=this.r2z*this.i2e21+-this.r2y*this.i2e22; this.ay2x=-this.r2z*this.i2e00+this.r2x*this.i2e02; this.ay2y=-this.r2z*this.i2e10+this.r2x*this.i2e12; this.ay2z=-this.r2z*this.i2e20+this.r2x*this.i2e22; this.az2x=this.r2y*this.i2e00+-this.r2x*this.i2e01; this.az2y=this.r2y*this.i2e10+-this.r2x*this.i2e11; this.az2z=this.r2y*this.i2e20+-this.r2x*this.i2e21; // calculate point-to-point mass matrix // from impulse equation // // M = ([/m] - [r^][/I][r^]) ^ -1 // // where // // [/m] = |1/m, 0, 0| // |0, 1/m, 0| // |0, 0, 1/m| // // [r^] = |0, -rz, ry| // |rz, 0, -rx| // |-ry, rx, 0| // // [/I] = Inverted moment inertia var k00=this.m1+this.m2; var k01=0; var k02=0; var k10=0; var k11=k00; var k12=0; var k20=0; var k21=0; var k22=k00; k00+=this.i1e11*this.r1z*this.r1z-(this.i1e21+this.i1e12)*this.r1y*this.r1z+this.i1e22*this.r1y*this.r1y; k01+=(this.i1e20*this.r1y+this.i1e12*this.r1x)*this.r1z-this.i1e10*this.r1z*this.r1z-this.i1e22*this.r1x*this.r1y; k02+=(this.i1e10*this.r1y-this.i1e11*this.r1x)*this.r1z-this.i1e20*this.r1y*this.r1y+this.i1e21*this.r1x*this.r1y; k10+=(this.i1e02*this.r1y+this.i1e21*this.r1x)*this.r1z-this.i1e01*this.r1z*this.r1z-this.i1e22*this.r1x*this.r1y; k11+=this.i1e00*this.r1z*this.r1z-(this.i1e20+this.i1e02)*this.r1x*this.r1z+this.i1e22*this.r1x*this.r1x; k12+=(this.i1e01*this.r1x-this.i1e00*this.r1y)*this.r1z-this.i1e21*this.r1x*this.r1x+this.i1e20*this.r1x*this.r1y; k20+=(this.i1e01*this.r1y-this.i1e11*this.r1x)*this.r1z-this.i1e02*this.r1y*this.r1y+this.i1e12*this.r1x*this.r1y; k21+=(this.i1e10*this.r1x-this.i1e00*this.r1y)*this.r1z-this.i1e12*this.r1x*this.r1x+this.i1e02*this.r1x*this.r1y; k22+=this.i1e00*this.r1y*this.r1y-(this.i1e10+this.i1e01)*this.r1x*this.r1y+this.i1e11*this.r1x*this.r1x; k00+=this.i2e11*this.r2z*this.r2z-(this.i2e21+this.i2e12)*this.r2y*this.r2z+this.i2e22*this.r2y*this.r2y; k01+=(this.i2e20*this.r2y+this.i2e12*this.r2x)*this.r2z-this.i2e10*this.r2z*this.r2z-this.i2e22*this.r2x*this.r2y; k02+=(this.i2e10*this.r2y-this.i2e11*this.r2x)*this.r2z-this.i2e20*this.r2y*this.r2y+this.i2e21*this.r2x*this.r2y; k10+=(this.i2e02*this.r2y+this.i2e21*this.r2x)*this.r2z-this.i2e01*this.r2z*this.r2z-this.i2e22*this.r2x*this.r2y; k11+=this.i2e00*this.r2z*this.r2z-(this.i2e20+this.i2e02)*this.r2x*this.r2z+this.i2e22*this.r2x*this.r2x; k12+=(this.i2e01*this.r2x-this.i2e00*this.r2y)*this.r2z-this.i2e21*this.r2x*this.r2x+this.i2e20*this.r2x*this.r2y; k20+=(this.i2e01*this.r2y-this.i2e11*this.r2x)*this.r2z-this.i2e02*this.r2y*this.r2y+this.i2e12*this.r2x*this.r2y; k21+=(this.i2e10*this.r2x-this.i2e00*this.r2y)*this.r2z-this.i2e12*this.r2x*this.r2x+this.i2e02*this.r2x*this.r2y; k22+=this.i2e00*this.r2y*this.r2y-(this.i2e10+this.i2e01)*this.r2x*this.r2y+this.i2e11*this.r2x*this.r2x; var inv=1/( k00*(k11*k22-k21*k12)+ k10*(k21*k02-k01*k22)+ k20*(k01*k12-k11*k02) ); this.d00=(k11*k22-k12*k21)*inv; this.d01=(k02*k21-k01*k22)*inv; this.d02=(k01*k12-k02*k11)*inv; this.d10=(k12*k20-k10*k22)*inv; this.d11=(k00*k22-k02*k20)*inv; this.d12=(k02*k10-k00*k12)*inv; this.d20=(k10*k21-k11*k20)*inv; this.d21=(k01*k20-k00*k21)*inv; this.d22=(k00*k11-k01*k10)*inv; this.velx=this.p2.x-this.p1.x; this.vely=this.p2.y-this.p1.y; this.velz=this.p2.z-this.p1.z; var len=Math.sqrt(this.velx*this.velx+this.vely*this.vely+this.velz*this.velz); if(len>0.005){ len=(0.005-len)/len*invTimeStep*0.05; this.velx*=len; this.vely*=len; this.velz*=len; }else{ this.velx=0; this.vely=0; this.velz=0; } this.impx*=0.95; this.impy*=0.95; this.impz*=0.95; this.l1.x+=this.impx*this.m1; this.l1.y+=this.impy*this.m1; this.l1.z+=this.impz*this.m1; this.a1.x+=this.impx*this.ax1x+this.impy*this.ay1x+this.impz*this.az1x; this.a1.y+=this.impx*this.ax1y+this.impy*this.ay1y+this.impz*this.az1y; this.a1.z+=this.impx*this.ax1z+this.impy*this.ay1z+this.impz*this.az1z; this.l2.x-=this.impx*this.m2; this.l2.y-=this.impy*this.m2; this.l2.z-=this.impz*this.m2; this.a2.x-=this.impx*this.ax2x+this.impy*this.ay2x+this.impz*this.az2x; this.a2.y-=this.impx*this.ax2y+this.impy*this.ay2y+this.impz*this.az2y; this.a2.z-=this.impx*this.ax2z+this.impy*this.ay2z+this.impz*this.az2z; }, solve:function(){ var rvx=this.l2.x-this.l1.x+this.a2.y*this.r2z-this.a2.z*this.r2y-this.a1.y*this.r1z+this.a1.z*this.r1y-this.velx; var rvy=this.l2.y-this.l1.y+this.a2.z*this.r2x-this.a2.x*this.r2z-this.a1.z*this.r1x+this.a1.x*this.r1z-this.vely; var rvz=this.l2.z-this.l1.z+this.a2.x*this.r2y-this.a2.y*this.r2x-this.a1.x*this.r1y+this.a1.y*this.r1x-this.velz; var nimpx=rvx*this.d00+rvy*this.d01+rvz*this.d02; var nimpy=rvx*this.d10+rvy*this.d11+rvz*this.d12; var nimpz=rvx*this.d20+rvy*this.d21+rvz*this.d22; this.impx+=nimpx; this.impy+=nimpy; this.impz+=nimpz; this.l1.x+=nimpx*this.m1; this.l1.y+=nimpy*this.m1; this.l1.z+=nimpz*this.m1; this.a1.x+=nimpx*this.ax1x+nimpy*this.ay1x+nimpz*this.az1x; this.a1.y+=nimpx*this.ax1y+nimpy*this.ay1y+nimpz*this.az1y; this.a1.z+=nimpx*this.ax1z+nimpy*this.ay1z+nimpz*this.az1z; this.l2.x-=nimpx*this.m2; this.l2.y-=nimpy*this.m2; this.l2.z-=nimpz*this.m2; this.a2.x-=nimpx*this.ax2x+nimpy*this.ay2x+nimpz*this.az2x; this.a2.y-=nimpx*this.ax2y+nimpy*this.ay2y+nimpz*this.az2y; this.a2.z-=nimpx*this.ax2z+nimpy*this.ay2z+nimpz*this.az2z; } } /** * A three-axis rotational constraint for various joints. * @author saharan */ OIMO.Rotational3Constraint = function(joint,limitMotor1,limitMotor2,limitMotor3){ this.cfm1=NaN; this.cfm2=NaN; this.cfm3=NaN; this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.ax1=NaN; this.ay1=NaN; this.az1=NaN; this.ax2=NaN; this.ay2=NaN; this.az2=NaN; this.ax3=NaN; this.ay3=NaN; this.az3=NaN; this.a1x1=NaN; // jacoians this.a1y1=NaN; this.a1z1=NaN; this.a2x1=NaN; this.a2y1=NaN; this.a2z1=NaN; this.a1x2=NaN; this.a1y2=NaN; this.a1z2=NaN; this.a2x2=NaN; this.a2y2=NaN; this.a2z2=NaN; this.a1x3=NaN; this.a1y3=NaN; this.a1z3=NaN; this.a2x3=NaN; this.a2y3=NaN; this.a2z3=NaN; this.lowerLimit1=NaN; this.upperLimit1=NaN; this.limitVelocity1=NaN; this.limitState1=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor1=false; this.motorSpeed1=NaN; this.maxMotorForce1=NaN; this.maxMotorImpulse1=NaN; this.lowerLimit2=NaN; this.upperLimit2=NaN; this.limitVelocity2=NaN; this.limitState2=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor2=false; this.motorSpeed2=NaN; this.maxMotorForce2=NaN; this.maxMotorImpulse2=NaN; this.lowerLimit3=NaN; this.upperLimit3=NaN; this.limitVelocity3=NaN; this.limitState3=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor3=false; this.motorSpeed3=NaN; this.maxMotorForce3=NaN; this.maxMotorImpulse3=NaN; this.k00=NaN; // K = J*M*JT this.k01=NaN; this.k02=NaN; this.k10=NaN; this.k11=NaN; this.k12=NaN; this.k20=NaN; this.k21=NaN; this.k22=NaN; this.kv00=NaN; // diagonals without CFMs this.kv11=NaN; this.kv22=NaN; this.dv00=NaN; // ...inverted this.dv11=NaN; this.dv22=NaN; this.d00=NaN; // K^-1 this.d01=NaN; this.d02=NaN; this.d10=NaN; this.d11=NaN; this.d12=NaN; this.d20=NaN; this.d21=NaN; this.d22=NaN; this.limitMotor1=limitMotor1; this.limitMotor2=limitMotor2; this.limitMotor3=limitMotor3; this.b1=joint.body1; this.b2=joint.body2; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.limitImpulse1=0; this.motorImpulse1=0; this.limitImpulse2=0; this.motorImpulse2=0; this.limitImpulse3=0; this.motorImpulse3=0; } OIMO.Rotational3Constraint.prototype = { constructor: OIMO.Rotational3Constraint, preSolve:function(timeStep,invTimeStep){ this.ax1=this.limitMotor1.axis.x; this.ay1=this.limitMotor1.axis.y; this.az1=this.limitMotor1.axis.z; this.ax2=this.limitMotor2.axis.x; this.ay2=this.limitMotor2.axis.y; this.az2=this.limitMotor2.axis.z; this.ax3=this.limitMotor3.axis.x; this.ay3=this.limitMotor3.axis.y; this.az3=this.limitMotor3.axis.z; this.lowerLimit1=this.limitMotor1.lowerLimit; this.upperLimit1=this.limitMotor1.upperLimit; this.motorSpeed1=this.limitMotor1.motorSpeed; this.maxMotorForce1=this.limitMotor1.maxMotorForce; this.enableMotor1=this.maxMotorForce1>0; this.lowerLimit2=this.limitMotor2.lowerLimit; this.upperLimit2=this.limitMotor2.upperLimit; this.motorSpeed2=this.limitMotor2.motorSpeed; this.maxMotorForce2=this.limitMotor2.maxMotorForce; this.enableMotor2=this.maxMotorForce2>0; this.lowerLimit3=this.limitMotor3.lowerLimit; this.upperLimit3=this.limitMotor3.upperLimit; this.motorSpeed3=this.limitMotor3.motorSpeed; this.maxMotorForce3=this.limitMotor3.maxMotorForce; this.enableMotor3=this.maxMotorForce3>0; var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; var frequency1=this.limitMotor1.frequency; var frequency2=this.limitMotor2.frequency; var frequency3=this.limitMotor3.frequency; var enableSpring1=frequency1>0; var enableSpring2=frequency2>0; var enableSpring3=frequency3>0; var enableLimit1=this.lowerLimit1<=this.upperLimit1; var enableLimit2=this.lowerLimit2<=this.upperLimit2; var enableLimit3=this.lowerLimit3<=this.upperLimit3; var angle1=this.limitMotor1.angle; if(enableLimit1){ if(this.lowerLimit1==this.upperLimit1){ if(this.limitState1!=0){ this.limitState1=0; this.limitImpulse1=0; } this.limitVelocity1=this.lowerLimit1-angle1; }else if(angle1this.upperLimit1){ if(this.limitState1!=1){ this.limitState1=1; this.limitImpulse1=0; } this.limitVelocity1=this.upperLimit1-angle1; }else{ this.limitState1=2; this.limitImpulse1=0; this.limitVelocity1=0; } if(!enableSpring1){ if(this.limitVelocity1>0.02)this.limitVelocity1-=0.02; else if(this.limitVelocity1<-0.02)this.limitVelocity1+=0.02; else this.limitVelocity1=0; } }else{ this.limitState1=2; this.limitImpulse1=0; } var angle2=this.limitMotor2.angle; if(enableLimit2){ if(this.lowerLimit2==this.upperLimit2){ if(this.limitState2!=0){ this.limitState2=0; this.limitImpulse2=0; } this.limitVelocity2=this.lowerLimit2-angle2; }else if(angle2this.upperLimit2){ if(this.limitState2!=1){ this.limitState2=1; this.limitImpulse2=0; } this.limitVelocity2=this.upperLimit2-angle2; }else{ this.limitState2=2; this.limitImpulse2=0; this.limitVelocity2=0; } if(!enableSpring2){ if(this.limitVelocity2>0.02)this.limitVelocity2-=0.02; else if(this.limitVelocity2<-0.02)this.limitVelocity2+=0.02; else this.limitVelocity2=0; } }else{ this.limitState2=2; this.limitImpulse2=0; } var angle3=this.limitMotor3.angle; if(enableLimit3){ if(this.lowerLimit3==this.upperLimit3){ if(this.limitState3!=0){ this.limitState3=0; this.limitImpulse3=0; } this.limitVelocity3=this.lowerLimit3-angle3; }else if(angle3this.upperLimit3){ if(this.limitState3!=1){ this.limitState3=1; this.limitImpulse3=0; } this.limitVelocity3=this.upperLimit3-angle3; }else{ this.limitState3=2; this.limitImpulse3=0; this.limitVelocity3=0; } if(!enableSpring3){ if(this.limitVelocity3>0.02)this.limitVelocity3-=0.02; else if(this.limitVelocity3<-0.02)this.limitVelocity3+=0.02; else this.limitVelocity3=0; } }else{ this.limitState3=2; this.limitImpulse3=0; } if(this.enableMotor1&&(this.limitState1!=0||enableSpring1)){ this.maxMotorImpulse1=this.maxMotorForce1*timeStep; }else{ this.motorImpulse1=0; this.maxMotorImpulse1=0; } if(this.enableMotor2&&(this.limitState2!=0||enableSpring2)){ this.maxMotorImpulse2=this.maxMotorForce2*timeStep; }else{ this.motorImpulse2=0; this.maxMotorImpulse2=0; } if(this.enableMotor3&&(this.limitState3!=0||enableSpring3)){ this.maxMotorImpulse3=this.maxMotorForce3*timeStep; }else{ this.motorImpulse3=0; this.maxMotorImpulse3=0; } // build jacobians this.a1x1=this.ax1*this.i1e00+this.ay1*this.i1e01+this.az1*this.i1e02; this.a1y1=this.ax1*this.i1e10+this.ay1*this.i1e11+this.az1*this.i1e12; this.a1z1=this.ax1*this.i1e20+this.ay1*this.i1e21+this.az1*this.i1e22; this.a2x1=this.ax1*this.i2e00+this.ay1*this.i2e01+this.az1*this.i2e02; this.a2y1=this.ax1*this.i2e10+this.ay1*this.i2e11+this.az1*this.i2e12; this.a2z1=this.ax1*this.i2e20+this.ay1*this.i2e21+this.az1*this.i2e22; this.a1x2=this.ax2*this.i1e00+this.ay2*this.i1e01+this.az2*this.i1e02; this.a1y2=this.ax2*this.i1e10+this.ay2*this.i1e11+this.az2*this.i1e12; this.a1z2=this.ax2*this.i1e20+this.ay2*this.i1e21+this.az2*this.i1e22; this.a2x2=this.ax2*this.i2e00+this.ay2*this.i2e01+this.az2*this.i2e02; this.a2y2=this.ax2*this.i2e10+this.ay2*this.i2e11+this.az2*this.i2e12; this.a2z2=this.ax2*this.i2e20+this.ay2*this.i2e21+this.az2*this.i2e22; this.a1x3=this.ax3*this.i1e00+this.ay3*this.i1e01+this.az3*this.i1e02; this.a1y3=this.ax3*this.i1e10+this.ay3*this.i1e11+this.az3*this.i1e12; this.a1z3=this.ax3*this.i1e20+this.ay3*this.i1e21+this.az3*this.i1e22; this.a2x3=this.ax3*this.i2e00+this.ay3*this.i2e01+this.az3*this.i2e02; this.a2y3=this.ax3*this.i2e10+this.ay3*this.i2e11+this.az3*this.i2e12; this.a2z3=this.ax3*this.i2e20+this.ay3*this.i2e21+this.az3*this.i2e22; // build an impulse matrix this.k00=this.ax1*(this.a1x1+this.a2x1)+this.ay1*(this.a1y1+this.a2y1)+this.az1*(this.a1z1+this.a2z1); this.k01=this.ax1*(this.a1x2+this.a2x2)+this.ay1*(this.a1y2+this.a2y2)+this.az1*(this.a1z2+this.a2z2); this.k02=this.ax1*(this.a1x3+this.a2x3)+this.ay1*(this.a1y3+this.a2y3)+this.az1*(this.a1z3+this.a2z3); this.k10=this.ax2*(this.a1x1+this.a2x1)+this.ay2*(this.a1y1+this.a2y1)+this.az2*(this.a1z1+this.a2z1); this.k11=this.ax2*(this.a1x2+this.a2x2)+this.ay2*(this.a1y2+this.a2y2)+this.az2*(this.a1z2+this.a2z2); this.k12=this.ax2*(this.a1x3+this.a2x3)+this.ay2*(this.a1y3+this.a2y3)+this.az2*(this.a1z3+this.a2z3); this.k20=this.ax3*(this.a1x1+this.a2x1)+this.ay3*(this.a1y1+this.a2y1)+this.az3*(this.a1z1+this.a2z1); this.k21=this.ax3*(this.a1x2+this.a2x2)+this.ay3*(this.a1y2+this.a2y2)+this.az3*(this.a1z2+this.a2z2); this.k22=this.ax3*(this.a1x3+this.a2x3)+this.ay3*(this.a1y3+this.a2y3)+this.az3*(this.a1z3+this.a2z3); this.kv00=this.k00; this.kv11=this.k11; this.kv22=this.k22; this.dv00=1/this.kv00; this.dv11=1/this.kv11; this.dv22=1/this.kv22; if(enableSpring1&&this.limitState1!=2){ var omega=6.2831853*frequency1; var k=omega*omega*timeStep; var dmp=invTimeStep/(k+2*this.limitMotor1.dampingRatio*omega); this.cfm1=this.kv00*dmp; this.limitVelocity1*=k*dmp; }else{ this.cfm1=0; this.limitVelocity1*=invTimeStep*0.05; } if(enableSpring2&&this.limitState2!=2){ omega=6.2831853*frequency2; k=omega*omega*timeStep; dmp=invTimeStep/(k+2*this.limitMotor2.dampingRatio*omega); this.cfm2=this.kv11*dmp; this.limitVelocity2*=k*dmp; }else{ this.cfm2=0; this.limitVelocity2*=invTimeStep*0.05; } if(enableSpring3&&this.limitState3!=2){ omega=6.2831853*frequency3; k=omega*omega*timeStep; dmp=invTimeStep/(k+2*this.limitMotor3.dampingRatio*omega); this.cfm3=this.kv22*dmp; this.limitVelocity3*=k*dmp; }else{ this.cfm3=0; this.limitVelocity3*=invTimeStep*0.05; } this.k00+=this.cfm1; this.k11+=this.cfm2; this.k22+=this.cfm3; var inv=1/( this.k00*(this.k11*this.k22-this.k21*this.k12)+ this.k10*(this.k21*this.k02-this.k01*this.k22)+ this.k20*(this.k01*this.k12-this.k11*this.k02) ); this.d00=(this.k11*this.k22-this.k12*this.k21)*inv; this.d01=(this.k02*this.k21-this.k01*this.k22)*inv; this.d02=(this.k01*this.k12-this.k02*this.k11)*inv; this.d10=(this.k12*this.k20-this.k10*this.k22)*inv; this.d11=(this.k00*this.k22-this.k02*this.k20)*inv; this.d12=(this.k02*this.k10-this.k00*this.k12)*inv; this.d20=(this.k10*this.k21-this.k11*this.k20)*inv; this.d21=(this.k01*this.k20-this.k00*this.k21)*inv; this.d22=(this.k00*this.k11-this.k01*this.k10)*inv; this.limitImpulse1*=0.95; this.motorImpulse1*=0.95; this.limitImpulse2*=0.95; this.motorImpulse2*=0.95; this.limitImpulse3*=0.95; this.motorImpulse3*=0.95; var totalImpulse1=this.limitImpulse1+this.motorImpulse1; var totalImpulse2=this.limitImpulse2+this.motorImpulse2; var totalImpulse3=this.limitImpulse3+this.motorImpulse3; this.a1.x+=totalImpulse1*this.a1x1+totalImpulse2*this.a1x2+totalImpulse3*this.a1x3; this.a1.y+=totalImpulse1*this.a1y1+totalImpulse2*this.a1y2+totalImpulse3*this.a1y3; this.a1.z+=totalImpulse1*this.a1z1+totalImpulse2*this.a1z2+totalImpulse3*this.a1z3; this.a2.x-=totalImpulse1*this.a2x1+totalImpulse2*this.a2x2+totalImpulse3*this.a2x3; this.a2.y-=totalImpulse1*this.a2y1+totalImpulse2*this.a2y2+totalImpulse3*this.a2y3; this.a2.z-=totalImpulse1*this.a2z1+totalImpulse2*this.a2z2+totalImpulse3*this.a2z3; }, solve_:function(){ var rvx=this.a2.x-this.a1.x; var rvy=this.a2.y-this.a1.y; var rvz=this.a2.z-this.a1.z; this.limitVelocity3=30; var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1-this.limitVelocity1; var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2-this.limitVelocity2; var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3-this.limitVelocity3; var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02; var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12; var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22; this.limitImpulse1+=dLimitImpulse1; this.limitImpulse2+=dLimitImpulse2; this.limitImpulse3+=dLimitImpulse3; this.a1.x+=dLimitImpulse1*this.a1x1+dLimitImpulse2*this.a1x2+dLimitImpulse3*this.a1x3; this.a1.y+=dLimitImpulse1*this.a1y1+dLimitImpulse2*this.a1y2+dLimitImpulse3*this.a1y3; this.a1.z+=dLimitImpulse1*this.a1z1+dLimitImpulse2*this.a1z2+dLimitImpulse3*this.a1z3; this.a2.x-=dLimitImpulse1*this.a2x1+dLimitImpulse2*this.a2x2+dLimitImpulse3*this.a2x3; this.a2.y-=dLimitImpulse1*this.a2y1+dLimitImpulse2*this.a2y2+dLimitImpulse3*this.a2y3; this.a2.z-=dLimitImpulse1*this.a2z1+dLimitImpulse2*this.a2z2+dLimitImpulse3*this.a2z3; }, solve:function(){ var rvx=this.a2.x-this.a1.x; var rvy=this.a2.y-this.a1.y; var rvz=this.a2.z-this.a1.z; var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1; var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2; var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3; var oldMotorImpulse1=this.motorImpulse1; var oldMotorImpulse2=this.motorImpulse2; var oldMotorImpulse3=this.motorImpulse3; var dMotorImpulse1=0; var dMotorImpulse2=0; var dMotorImpulse3=0; if(this.enableMotor1){ dMotorImpulse1=(rvn1-this.motorSpeed1)*this.dv00; this.motorImpulse1+=dMotorImpulse1; if(this.motorImpulse1>this.maxMotorImpulse1){ // clamp motor impulse this.motorImpulse1=this.maxMotorImpulse1; }else if(this.motorImpulse1<-this.maxMotorImpulse1){ this.motorImpulse1=-this.maxMotorImpulse1; } dMotorImpulse1=this.motorImpulse1-oldMotorImpulse1; } if(this.enableMotor2){ dMotorImpulse2=(rvn2-this.motorSpeed2)*this.dv11; this.motorImpulse2+=dMotorImpulse2; if(this.motorImpulse2>this.maxMotorImpulse2){ // clamp motor impulse this.motorImpulse2=this.maxMotorImpulse2; }else if(this.motorImpulse2<-this.maxMotorImpulse2){ this.motorImpulse2=-this.maxMotorImpulse2; } dMotorImpulse2=this.motorImpulse2-oldMotorImpulse2; } if(this.enableMotor3){ dMotorImpulse3=(rvn3-this.motorSpeed3)*this.dv22; this.motorImpulse3+=dMotorImpulse3; if(this.motorImpulse3>this.maxMotorImpulse3){ // clamp motor impulse this.motorImpulse3=this.maxMotorImpulse3; }else if(this.motorImpulse3<-this.maxMotorImpulse3){ this.motorImpulse3=-this.maxMotorImpulse3; } dMotorImpulse3=this.motorImpulse3-oldMotorImpulse3; } // apply motor impulse to relative velocity rvn1+=dMotorImpulse1*this.kv00+dMotorImpulse2*this.k01+dMotorImpulse3*this.k02; rvn2+=dMotorImpulse1*this.k10+dMotorImpulse2*this.kv11+dMotorImpulse3*this.k12; rvn3+=dMotorImpulse1*this.k20+dMotorImpulse2*this.k21+dMotorImpulse3*this.kv22; // subtract target velocity and applied impulse rvn1-=this.limitVelocity1+this.limitImpulse1*this.cfm1; rvn2-=this.limitVelocity2+this.limitImpulse2*this.cfm2; rvn3-=this.limitVelocity3+this.limitImpulse3*this.cfm3; var oldLimitImpulse1=this.limitImpulse1; var oldLimitImpulse2=this.limitImpulse2; var oldLimitImpulse3=this.limitImpulse3; var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02; var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12; var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22; this.limitImpulse1+=dLimitImpulse1; this.limitImpulse2+=dLimitImpulse2; this.limitImpulse3+=dLimitImpulse3; // clamp var clampState=0; if(this.limitState1==2||this.limitImpulse1*this.limitState1<0){ dLimitImpulse1=-oldLimitImpulse1; rvn2+=dLimitImpulse1*this.k10; rvn3+=dLimitImpulse1*this.k20; clampState|=1; } if(this.limitState2==2||this.limitImpulse2*this.limitState2<0){ dLimitImpulse2=-oldLimitImpulse2; rvn1+=dLimitImpulse2*this.k01; rvn3+=dLimitImpulse2*this.k21; clampState|=2; } if(this.limitState3==2||this.limitImpulse3*this.limitState3<0){ dLimitImpulse3=-oldLimitImpulse3; rvn1+=dLimitImpulse3*this.k02; rvn2+=dLimitImpulse3*this.k12; clampState|=4; } // update un-clamped impulse // TODO: isolate division var det; switch(clampState){ case 1: // update 2 3 det=1/(this.k11*this.k22-this.k12*this.k21); dLimitImpulse2=(this.k22*rvn2+-this.k12*rvn3)*det; dLimitImpulse3=(-this.k21*rvn2+this.k11*rvn3)*det; break; case 2: // update 1 3 det=1/(this.k00*this.k22-this.k02*this.k20); dLimitImpulse1=(this.k22*rvn1+-this.k02*rvn3)*det; dLimitImpulse3=(-this.k20*rvn1+this.k00*rvn3)*det; break; case 3: // update 3 dLimitImpulse3=rvn3/this.k22; break; case 4: // update 1 2 det=1/(this.k00*this.k11-this.k01*this.k10); dLimitImpulse1=(this.k11*rvn1+-this.k01*rvn2)*det; dLimitImpulse2=(-this.k10*rvn1+this.k00*rvn2)*det; break; case 5: // update 2 dLimitImpulse2=rvn2/this.k11; break; case 6: // update 1 dLimitImpulse1=rvn1/this.k00; break; } this.limitImpulse1=dLimitImpulse1+oldLimitImpulse1; this.limitImpulse2=dLimitImpulse2+oldLimitImpulse2; this.limitImpulse3=dLimitImpulse3+oldLimitImpulse3; var dImpulse1=dMotorImpulse1+dLimitImpulse1; var dImpulse2=dMotorImpulse2+dLimitImpulse2; var dImpulse3=dMotorImpulse3+dLimitImpulse3; // apply impulse this.a1.x+=dImpulse1*this.a1x1+dImpulse2*this.a1x2+dImpulse3*this.a1x3; this.a1.y+=dImpulse1*this.a1y1+dImpulse2*this.a1y2+dImpulse3*this.a1y3; this.a1.z+=dImpulse1*this.a1z1+dImpulse2*this.a1z2+dImpulse3*this.a1z3; this.a2.x-=dImpulse1*this.a2x1+dImpulse2*this.a2x2+dImpulse3*this.a2x3; this.a2.y-=dImpulse1*this.a2y1+dImpulse2*this.a2y2+dImpulse3*this.a2y3; this.a2.z-=dImpulse1*this.a2z1+dImpulse2*this.a2z2+dImpulse3*this.a2z3; rvx=this.a2.x-this.a1.x; rvy=this.a2.y-this.a1.y; rvz=this.a2.z-this.a1.z; rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2; } } /** * A rotational constraint for various joints. * @author saharan */ OIMO.RotationalConstraint = function(joint,limitMotor){ this.cfm=NaN; this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.motorDenom=NaN; this.invMotorDenom=NaN; this.invDenom=NaN; this.ax=NaN; this.ay=NaN; this.az=NaN; this.a1x=NaN; this.a1y=NaN; this.a1z=NaN; this.a2x=NaN; this.a2y=NaN; this.a2z=NaN; this.enableLimit=false; this.lowerLimit=NaN; this.upperLimit=NaN; this.limitVelocity=NaN; this.limitState=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor=false; this.motorSpeed=NaN; this.maxMotorForce=NaN; this.maxMotorImpulse=NaN; this.limitMotor=limitMotor; this.b1=joint.body1; this.b2=joint.body2; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.limitImpulse=0; this.motorImpulse=0; } OIMO.RotationalConstraint.prototype = { constructor: OIMO.RotationalConstraint, preSolve:function(timeStep,invTimeStep){ this.ax=this.limitMotor.axis.x; this.ay=this.limitMotor.axis.y; this.az=this.limitMotor.axis.z; this.lowerLimit=this.limitMotor.lowerLimit; this.upperLimit=this.limitMotor.upperLimit; this.motorSpeed=this.limitMotor.motorSpeed; this.maxMotorForce=this.limitMotor.maxMotorForce; this.enableMotor=this.maxMotorForce>0; var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; var frequency=this.limitMotor.frequency; var enableSpring=frequency>0; var enableLimit=this.lowerLimit<=this.upperLimit; var angle=this.limitMotor.angle; if(enableLimit){ if(this.lowerLimit==this.upperLimit){ if(this.limitState!=0){ this.limitState=0; this.limitImpulse=0; } this.limitVelocity=this.lowerLimit-angle; }else if(anglethis.upperLimit){ if(this.limitState!=1){ this.limitState=1; this.limitImpulse=0; } this.limitVelocity=this.upperLimit-angle; }else{ this.limitState=2; this.limitImpulse=0; this.limitVelocity=0; } if(!enableSpring){ if(this.limitVelocity>0.02)this.limitVelocity-=0.02; else if(this.limitVelocity<-0.02)this.limitVelocity+=0.02; else this.limitVelocity=0; } }else{ this.limitState=2; this.limitImpulse=0; } if(this.enableMotor&&(this.limitState!=0||enableSpring)){ this.maxMotorImpulse=this.maxMotorForce*timeStep; }else{ this.motorImpulse=0; this.maxMotorImpulse=0; } this.a1x=this.ax*this.i1e00+this.ay*this.i1e01+this.az*this.i1e02; this.a1y=this.ax*this.i1e10+this.ay*this.i1e11+this.az*this.i1e12; this.a1z=this.ax*this.i1e20+this.ay*this.i1e21+this.az*this.i1e22; this.a2x=this.ax*this.i2e00+this.ay*this.i2e01+this.az*this.i2e02; this.a2y=this.ax*this.i2e10+this.ay*this.i2e11+this.az*this.i2e12; this.a2z=this.ax*this.i2e20+this.ay*this.i2e21+this.az*this.i2e22; this.motorDenom=this.ax*(this.a1x+this.a2x)+this.ay*(this.a1y+this.a2y)+this.az*(this.a1z+this.a2z); this.invMotorDenom=1/this.motorDenom; if(enableSpring&&this.limitState!=2){ var omega=6.2831853*frequency; var k=omega*omega*timeStep; var dmp=invTimeStep/(k+2*this.limitMotor.dampingRatio*omega); this.cfm=this.motorDenom*dmp; this.limitVelocity*=k*dmp; }else{ this.cfm=0; this.limitVelocity*=invTimeStep*0.05; } this.invDenom=1/(this.motorDenom+this.cfm); this.limitImpulse*=0.95; this.motorImpulse*=0.95; var totalImpulse=this.limitImpulse+this.motorImpulse; this.a1.x+=totalImpulse*this.a1x; this.a1.y+=totalImpulse*this.a1y; this.a1.z+=totalImpulse*this.a1z; this.a2.x-=totalImpulse*this.a2x; this.a2.y-=totalImpulse*this.a2y; this.a2.z-=totalImpulse*this.a2z; }, solve:function(){ var rvn=this.ax*(this.a2.x-this.a1.x)+this.ay*(this.a2.y-this.a1.y)+this.az*(this.a2.z-this.a1.z); // motor part var newMotorImpulse; if(this.enableMotor){ newMotorImpulse=(rvn-this.motorSpeed)*this.invMotorDenom; var oldMotorImpulse=this.motorImpulse; this.motorImpulse+=newMotorImpulse; if(this.motorImpulse>this.maxMotorImpulse)this.motorImpulse=this.maxMotorImpulse; else if(this.motorImpulse<-this.maxMotorImpulse)this.motorImpulse=-this.maxMotorImpulse; newMotorImpulse=this.motorImpulse-oldMotorImpulse; rvn-=newMotorImpulse*this.motorDenom; }else newMotorImpulse=0; // limit part var newLimitImpulse; if(this.limitState!=2){ newLimitImpulse=(rvn-this.limitVelocity-this.limitImpulse*this.cfm)*this.invDenom; var oldLimitImpulse=this.limitImpulse; this.limitImpulse+=newLimitImpulse; if(this.limitImpulse*this.limitState<0)this.limitImpulse=0; newLimitImpulse=this.limitImpulse-oldLimitImpulse; }else newLimitImpulse=0; var totalImpulse=newLimitImpulse+newMotorImpulse; this.a1.x+=totalImpulse*this.a1x; this.a1.y+=totalImpulse*this.a1y; this.a1.z+=totalImpulse*this.a1z; this.a2.x-=totalImpulse*this.a2x; this.a2.y-=totalImpulse*this.a2y; this.a2.z-=totalImpulse*this.a2z; } } /** * A three-axis translational constraint for various joints. * @author saharan */ OIMO.Translational3Constraint = function(joint,limitMotor1,limitMotor2,limitMotor3){ this.m1=NaN; this.m2=NaN; this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.ax1=NaN; this.ay1=NaN; this.az1=NaN; this.ax2=NaN; this.ay2=NaN; this.az2=NaN; this.ax3=NaN; this.ay3=NaN; this.az3=NaN; this.r1x=NaN; this.r1y=NaN; this.r1z=NaN; this.r2x=NaN; this.r2y=NaN; this.r2z=NaN; this.t1x1=NaN;// jacobians this.t1y1=NaN; this.t1z1=NaN; this.t2x1=NaN; this.t2y1=NaN; this.t2z1=NaN; this.l1x1=NaN; this.l1y1=NaN; this.l1z1=NaN; this.l2x1=NaN; this.l2y1=NaN; this.l2z1=NaN; this.a1x1=NaN; this.a1y1=NaN; this.a1z1=NaN; this.a2x1=NaN; this.a2y1=NaN; this.a2z1=NaN; this.t1x2=NaN; this.t1y2=NaN; this.t1z2=NaN; this.t2x2=NaN; this.t2y2=NaN; this.t2z2=NaN; this.l1x2=NaN; this.l1y2=NaN; this.l1z2=NaN; this.l2x2=NaN; this.l2y2=NaN; this.l2z2=NaN; this.a1x2=NaN; this.a1y2=NaN; this.a1z2=NaN; this.a2x2=NaN; this.a2y2=NaN; this.a2z2=NaN; this.t1x3=NaN; this.t1y3=NaN; this.t1z3=NaN; this.t2x3=NaN; this.t2y3=NaN; this.t2z3=NaN; this.l1x3=NaN; this.l1y3=NaN; this.l1z3=NaN; this.l2x3=NaN; this.l2y3=NaN; this.l2z3=NaN; this.a1x3=NaN; this.a1y3=NaN; this.a1z3=NaN; this.a2x3=NaN; this.a2y3=NaN; this.a2z3=NaN; this.lowerLimit1=NaN; this.upperLimit1=NaN; this.limitVelocity1=NaN; this.limitState1=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited this.enableMotor1=false; this.motorSpeed1=NaN; this.maxMotorForce1=NaN; this.maxMotorImpulse1=NaN; this.lowerLimit2=NaN; this.upperLimit2=NaN; this.limitVelocity2=NaN; this.limitState2=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited this.enableMotor2=false; this.motorSpeed2=NaN; this.maxMotorForce2=NaN; this.maxMotorImpulse2=NaN; this.lowerLimit3=NaN; this.upperLimit3=NaN; this.limitVelocity3=NaN; this.limitState3=0; // -1: at lower, 0: locked, 1: at upper, 2: unlimited this.enableMotor3=false; this.motorSpeed3=NaN; this.maxMotorForce3=NaN; this.maxMotorImpulse3=NaN; this.k00=NaN; // K = J*M*JT this.k01=NaN; this.k02=NaN; this.k10=NaN; this.k11=NaN; this.k12=NaN; this.k20=NaN; this.k21=NaN; this.k22=NaN; this.kv00=NaN; // diagonals without CFMs this.kv11=NaN; this.kv22=NaN; this.dv00=NaN; // ...inverted this.dv11=NaN; this.dv22=NaN; this.d00=NaN; // K^-1 this.d01=NaN; this.d02=NaN; this.d10=NaN; this.d11=NaN; this.d12=NaN; this.d20=NaN; this.d21=NaN; this.d22=NaN; this.limitMotor1=limitMotor1; this.limitMotor2=limitMotor2; this.limitMotor3=limitMotor3; this.b1=joint.body1; this.b2=joint.body2; this.p1=joint.anchorPoint1; this.p2=joint.anchorPoint2; this.r1=joint.relativeAnchorPoint1; this.r2=joint.relativeAnchorPoint2; this.l1=this.b1.linearVelocity; this.l2=this.b2.linearVelocity; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.limitImpulse1=0; this.motorImpulse1=0; this.limitImpulse2=0; this.motorImpulse2=0; this.limitImpulse3=0; this.motorImpulse3=0; this.cfm1=0;// Constraint Force Mixing this.cfm2=0; this.cfm3=0; this.weight=-1; } OIMO.Translational3Constraint.prototype = { constructor: OIMO.Translational3Constraint, preSolve:function(timeStep,invTimeStep){ this.ax1=this.limitMotor1.axis.x; this.ay1=this.limitMotor1.axis.y; this.az1=this.limitMotor1.axis.z; this.ax2=this.limitMotor2.axis.x; this.ay2=this.limitMotor2.axis.y; this.az2=this.limitMotor2.axis.z; this.ax3=this.limitMotor3.axis.x; this.ay3=this.limitMotor3.axis.y; this.az3=this.limitMotor3.axis.z; this.lowerLimit1=this.limitMotor1.lowerLimit; this.upperLimit1=this.limitMotor1.upperLimit; this.motorSpeed1=this.limitMotor1.motorSpeed; this.maxMotorForce1=this.limitMotor1.maxMotorForce; this.enableMotor1=this.maxMotorForce1>0; this.lowerLimit2=this.limitMotor2.lowerLimit; this.upperLimit2=this.limitMotor2.upperLimit; this.motorSpeed2=this.limitMotor2.motorSpeed; this.maxMotorForce2=this.limitMotor2.maxMotorForce; this.enableMotor2=this.maxMotorForce2>0; this.lowerLimit3=this.limitMotor3.lowerLimit; this.upperLimit3=this.limitMotor3.upperLimit; this.motorSpeed3=this.limitMotor3.motorSpeed; this.maxMotorForce3=this.limitMotor3.maxMotorForce; this.enableMotor3=this.maxMotorForce3>0; this.m1=this.b1.inverseMass; this.m2=this.b2.inverseMass; var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; var dx=this.p2.x-this.p1.x; var dy=this.p2.y-this.p1.y; var dz=this.p2.z-this.p1.z; var d1=dx*this.ax1+dy*this.ay1+dz*this.az1; var d2=dx*this.ax2+dy*this.ay2+dz*this.az2; var d3=dx*this.ax3+dy*this.ay3+dz*this.az3; var frequency1=this.limitMotor1.frequency; var frequency2=this.limitMotor2.frequency; var frequency3=this.limitMotor3.frequency; var enableSpring1=frequency1>0; var enableSpring2=frequency2>0; var enableSpring3=frequency3>0; var enableLimit1=this.lowerLimit1<=this.upperLimit1; var enableLimit2=this.lowerLimit2<=this.upperLimit2; var enableLimit3=this.lowerLimit3<=this.upperLimit3; // for stability if(enableSpring1&&d1>20||d1<-20){ enableSpring1=false; } if(enableSpring2&&d2>20||d2<-20){ enableSpring2=false; } if(enableSpring3&&d3>20||d3<-20){ enableSpring3=false; } if(enableLimit1){ if(this.lowerLimit1==this.upperLimit1){ if(this.limitState1!=0){ this.limitState1=0; this.limitImpulse1=0; } this.limitVelocity1=this.lowerLimit1-d1; if(!enableSpring1)d1=this.lowerLimit1; }else if(d1this.upperLimit1){ if(this.limitState1!=1){ this.limitState1=1; this.limitImpulse1=0; } this.limitVelocity1=this.upperLimit1-d1; if(!enableSpring1)d1=this.upperLimit1; }else{ this.limitState1=2; this.limitImpulse1=0; this.limitVelocity1=0; } if(!enableSpring1){ if(this.limitVelocity1>0.005)this.limitVelocity1-=0.005; else if(this.limitVelocity1<-0.005)this.limitVelocity1+=0.005; else this.limitVelocity1=0; } }else{ this.limitState1=2; this.limitImpulse1=0; } if(enableLimit2){ if(this.lowerLimit2==this.upperLimit2){ if(this.limitState2!=0){ this.limitState2=0; this.limitImpulse2=0; } this.limitVelocity2=this.lowerLimit2-d2; if(!enableSpring2)d2=this.lowerLimit2; }else if(d2this.upperLimit2){ if(this.limitState2!=1){ this.limitState2=1; this.limitImpulse2=0; } this.limitVelocity2=this.upperLimit2-d2; if(!enableSpring2)d2=this.upperLimit2; }else{ this.limitState2=2; this.limitImpulse2=0; this.limitVelocity2=0; } if(!enableSpring2){ if(this.limitVelocity2>0.005)this.limitVelocity2-=0.005; else if(this.limitVelocity2<-0.005)this.limitVelocity2+=0.005; else this.limitVelocity2=0; } }else{ this.limitState2=2; this.limitImpulse2=0; } if(enableLimit3){ if(this.lowerLimit3==this.upperLimit3){ if(this.limitState3!=0){ this.limitState3=0; this.limitImpulse3=0; } this.limitVelocity3=this.lowerLimit3-d3; if(!enableSpring3)d3=this.lowerLimit3; }else if(d3this.upperLimit3){ if(this.limitState3!=1){ this.limitState3=1; this.limitImpulse3=0; } this.limitVelocity3=this.upperLimit3-d3; if(!enableSpring3)d3=this.upperLimit3; }else{ this.limitState3=2; this.limitImpulse3=0; this.limitVelocity3=0; } if(!enableSpring3){ if(this.limitVelocity3>0.005)this.limitVelocity3-=0.005; else if(this.limitVelocity3<-0.005)this.limitVelocity3+=0.005; else this.limitVelocity3=0; } }else{ this.limitState3=2; this.limitImpulse3=0; } if(this.enableMotor1&&(this.limitState1!=0||enableSpring1)){ this.maxMotorImpulse1=this.maxMotorForce1*timeStep; }else{ this.motorImpulse1=0; this.maxMotorImpulse1=0; } if(this.enableMotor2&&(this.limitState2!=0||enableSpring2)){ this.maxMotorImpulse2=this.maxMotorForce2*timeStep; }else{ this.motorImpulse2=0; this.maxMotorImpulse2=0; } if(this.enableMotor3&&(this.limitState3!=0||enableSpring3)){ this.maxMotorImpulse3=this.maxMotorForce3*timeStep; }else{ this.motorImpulse3=0; this.maxMotorImpulse3=0; } var rdx=d1*this.ax1+d2*this.ax2+d3*this.ax2; var rdy=d1*this.ay1+d2*this.ay2+d3*this.ay2; var rdz=d1*this.az1+d2*this.az2+d3*this.az2; var w1=this.m2/(this.m1+this.m2); if(this.weight>=0)w1=this.weight; // use given weight var w2=1-w1; this.r1x=this.r1.x+rdx*w1; this.r1y=this.r1.y+rdy*w1; this.r1z=this.r1.z+rdz*w1; this.r2x=this.r2.x-rdx*w2; this.r2y=this.r2.y-rdy*w2; this.r2z=this.r2.z-rdz*w2; // build jacobians this.t1x1=this.r1y*this.az1-this.r1z*this.ay1; this.t1y1=this.r1z*this.ax1-this.r1x*this.az1; this.t1z1=this.r1x*this.ay1-this.r1y*this.ax1; this.t2x1=this.r2y*this.az1-this.r2z*this.ay1; this.t2y1=this.r2z*this.ax1-this.r2x*this.az1; this.t2z1=this.r2x*this.ay1-this.r2y*this.ax1; this.l1x1=this.ax1*this.m1; this.l1y1=this.ay1*this.m1; this.l1z1=this.az1*this.m1; this.l2x1=this.ax1*this.m2; this.l2y1=this.ay1*this.m2; this.l2z1=this.az1*this.m2; this.a1x1=this.t1x1*this.i1e00+this.t1y1*this.i1e01+this.t1z1*this.i1e02; this.a1y1=this.t1x1*this.i1e10+this.t1y1*this.i1e11+this.t1z1*this.i1e12; this.a1z1=this.t1x1*this.i1e20+this.t1y1*this.i1e21+this.t1z1*this.i1e22; this.a2x1=this.t2x1*this.i2e00+this.t2y1*this.i2e01+this.t2z1*this.i2e02; this.a2y1=this.t2x1*this.i2e10+this.t2y1*this.i2e11+this.t2z1*this.i2e12; this.a2z1=this.t2x1*this.i2e20+this.t2y1*this.i2e21+this.t2z1*this.i2e22; this.t1x2=this.r1y*this.az2-this.r1z*this.ay2; this.t1y2=this.r1z*this.ax2-this.r1x*this.az2; this.t1z2=this.r1x*this.ay2-this.r1y*this.ax2; this.t2x2=this.r2y*this.az2-this.r2z*this.ay2; this.t2y2=this.r2z*this.ax2-this.r2x*this.az2; this.t2z2=this.r2x*this.ay2-this.r2y*this.ax2; this.l1x2=this.ax2*this.m1; this.l1y2=this.ay2*this.m1; this.l1z2=this.az2*this.m1; this.l2x2=this.ax2*this.m2; this.l2y2=this.ay2*this.m2; this.l2z2=this.az2*this.m2; this.a1x2=this.t1x2*this.i1e00+this.t1y2*this.i1e01+this.t1z2*this.i1e02; this.a1y2=this.t1x2*this.i1e10+this.t1y2*this.i1e11+this.t1z2*this.i1e12; this.a1z2=this.t1x2*this.i1e20+this.t1y2*this.i1e21+this.t1z2*this.i1e22; this.a2x2=this.t2x2*this.i2e00+this.t2y2*this.i2e01+this.t2z2*this.i2e02; this.a2y2=this.t2x2*this.i2e10+this.t2y2*this.i2e11+this.t2z2*this.i2e12; this.a2z2=this.t2x2*this.i2e20+this.t2y2*this.i2e21+this.t2z2*this.i2e22; this.t1x3=this.r1y*this.az3-this.r1z*this.ay3; this.t1y3=this.r1z*this.ax3-this.r1x*this.az3; this.t1z3=this.r1x*this.ay3-this.r1y*this.ax3; this.t2x3=this.r2y*this.az3-this.r2z*this.ay3; this.t2y3=this.r2z*this.ax3-this.r2x*this.az3; this.t2z3=this.r2x*this.ay3-this.r2y*this.ax3; this.l1x3=this.ax3*this.m1; this.l1y3=this.ay3*this.m1; this.l1z3=this.az3*this.m1; this.l2x3=this.ax3*this.m2; this.l2y3=this.ay3*this.m2; this.l2z3=this.az3*this.m2; this.a1x3=this.t1x3*this.i1e00+this.t1y3*this.i1e01+this.t1z3*this.i1e02; this.a1y3=this.t1x3*this.i1e10+this.t1y3*this.i1e11+this.t1z3*this.i1e12; this.a1z3=this.t1x3*this.i1e20+this.t1y3*this.i1e21+this.t1z3*this.i1e22; this.a2x3=this.t2x3*this.i2e00+this.t2y3*this.i2e01+this.t2z3*this.i2e02; this.a2y3=this.t2x3*this.i2e10+this.t2y3*this.i2e11+this.t2z3*this.i2e12; this.a2z3=this.t2x3*this.i2e20+this.t2y3*this.i2e21+this.t2z3*this.i2e22; // build an impulse matrix var m12=this.m1+this.m2; this.k00=(this.ax1*this.ax1+this.ay1*this.ay1+this.az1*this.az1)*m12; this.k01=(this.ax1*this.ax2+this.ay1*this.ay2+this.az1*this.az2)*m12; this.k02=(this.ax1*this.ax3+this.ay1*this.ay3+this.az1*this.az3)*m12; this.k10=(this.ax2*this.ax1+this.ay2*this.ay1+this.az2*this.az1)*m12; this.k11=(this.ax2*this.ax2+this.ay2*this.ay2+this.az2*this.az2)*m12; this.k12=(this.ax2*this.ax3+this.ay2*this.ay3+this.az2*this.az3)*m12; this.k20=(this.ax3*this.ax1+this.ay3*this.ay1+this.az3*this.az1)*m12; this.k21=(this.ax3*this.ax2+this.ay3*this.ay2+this.az3*this.az2)*m12; this.k22=(this.ax3*this.ax3+this.ay3*this.ay3+this.az3*this.az3)*m12; this.k00+=this.t1x1*this.a1x1+this.t1y1*this.a1y1+this.t1z1*this.a1z1; this.k01+=this.t1x1*this.a1x2+this.t1y1*this.a1y2+this.t1z1*this.a1z2; this.k02+=this.t1x1*this.a1x3+this.t1y1*this.a1y3+this.t1z1*this.a1z3; this.k10+=this.t1x2*this.a1x1+this.t1y2*this.a1y1+this.t1z2*this.a1z1; this.k11+=this.t1x2*this.a1x2+this.t1y2*this.a1y2+this.t1z2*this.a1z2; this.k12+=this.t1x2*this.a1x3+this.t1y2*this.a1y3+this.t1z2*this.a1z3; this.k20+=this.t1x3*this.a1x1+this.t1y3*this.a1y1+this.t1z3*this.a1z1; this.k21+=this.t1x3*this.a1x2+this.t1y3*this.a1y2+this.t1z3*this.a1z2; this.k22+=this.t1x3*this.a1x3+this.t1y3*this.a1y3+this.t1z3*this.a1z3; this.k00+=this.t2x1*this.a2x1+this.t2y1*this.a2y1+this.t2z1*this.a2z1; this.k01+=this.t2x1*this.a2x2+this.t2y1*this.a2y2+this.t2z1*this.a2z2; this.k02+=this.t2x1*this.a2x3+this.t2y1*this.a2y3+this.t2z1*this.a2z3; this.k10+=this.t2x2*this.a2x1+this.t2y2*this.a2y1+this.t2z2*this.a2z1; this.k11+=this.t2x2*this.a2x2+this.t2y2*this.a2y2+this.t2z2*this.a2z2; this.k12+=this.t2x2*this.a2x3+this.t2y2*this.a2y3+this.t2z2*this.a2z3; this.k20+=this.t2x3*this.a2x1+this.t2y3*this.a2y1+this.t2z3*this.a2z1; this.k21+=this.t2x3*this.a2x2+this.t2y3*this.a2y2+this.t2z3*this.a2z2; this.k22+=this.t2x3*this.a2x3+this.t2y3*this.a2y3+this.t2z3*this.a2z3; this.kv00=this.k00; this.kv11=this.k11; this.kv22=this.k22; this.dv00=1/this.kv00; this.dv11=1/this.kv11; this.dv22=1/this.kv22; if(enableSpring1&&this.limitState1!=2){ var omega=6.2831853*frequency1; var k=omega*omega*timeStep; var dmp=invTimeStep/(k+2*this.limitMotor1.dampingRatio*omega); this.cfm1=this.kv00*dmp; this.limitVelocity1*=k*dmp; }else{ this.cfm1=0; this.limitVelocity1*=invTimeStep*0.05; } if(enableSpring2&&this.limitState2!=2){ omega=6.2831853*frequency2; k=omega*omega*timeStep; dmp=invTimeStep/(k+2*this.limitMotor2.dampingRatio*omega); this.cfm2=this.kv11*dmp; this.limitVelocity2*=k*dmp; }else{ this.cfm2=0; this.limitVelocity2*=invTimeStep*0.05; } if(enableSpring3&&this.limitState3!=2){ omega=6.2831853*frequency3; k=omega*omega*timeStep; dmp=invTimeStep/(k+2*this.limitMotor3.dampingRatio*omega); this.cfm3=this.kv22*dmp; this.limitVelocity3*=k*dmp; }else{ this.cfm3=0; this.limitVelocity3*=invTimeStep*0.05; } this.k00+=this.cfm1; this.k11+=this.cfm2; this.k22+=this.cfm3; var inv=1/( this.k00*(this.k11*this.k22-this.k21*this.k12)+ this.k10*(this.k21*this.k02-this.k01*this.k22)+ this.k20*(this.k01*this.k12-this.k11*this.k02) ); this.d00=(this.k11*this.k22-this.k12*this.k21)*inv; this.d01=(this.k02*this.k21-this.k01*this.k22)*inv; this.d02=(this.k01*this.k12-this.k02*this.k11)*inv; this.d10=(this.k12*this.k20-this.k10*this.k22)*inv; this.d11=(this.k00*this.k22-this.k02*this.k20)*inv; this.d12=(this.k02*this.k10-this.k00*this.k12)*inv; this.d20=(this.k10*this.k21-this.k11*this.k20)*inv; this.d21=(this.k01*this.k20-this.k00*this.k21)*inv; this.d22=(this.k00*this.k11-this.k01*this.k10)*inv; // warm starting var totalImpulse1=this.limitImpulse1+this.motorImpulse1; var totalImpulse2=this.limitImpulse2+this.motorImpulse2; var totalImpulse3=this.limitImpulse3+this.motorImpulse3; this.l1.x+=totalImpulse1*this.l1x1+totalImpulse2*this.l1x2+totalImpulse3*this.l1x3; this.l1.y+=totalImpulse1*this.l1y1+totalImpulse2*this.l1y2+totalImpulse3*this.l1y3; this.l1.z+=totalImpulse1*this.l1z1+totalImpulse2*this.l1z2+totalImpulse3*this.l1z3; this.a1.x+=totalImpulse1*this.a1x1+totalImpulse2*this.a1x2+totalImpulse3*this.a1x3; this.a1.y+=totalImpulse1*this.a1y1+totalImpulse2*this.a1y2+totalImpulse3*this.a1y3; this.a1.z+=totalImpulse1*this.a1z1+totalImpulse2*this.a1z2+totalImpulse3*this.a1z3; this.l2.x-=totalImpulse1*this.l2x1+totalImpulse2*this.l2x2+totalImpulse3*this.l2x3; this.l2.y-=totalImpulse1*this.l2y1+totalImpulse2*this.l2y2+totalImpulse3*this.l2y3; this.l2.z-=totalImpulse1*this.l2z1+totalImpulse2*this.l2z2+totalImpulse3*this.l2z3; this.a2.x-=totalImpulse1*this.a2x1+totalImpulse2*this.a2x2+totalImpulse3*this.a2x3; this.a2.y-=totalImpulse1*this.a2y1+totalImpulse2*this.a2y2+totalImpulse3*this.a2y3; this.a2.z-=totalImpulse1*this.a2z1+totalImpulse2*this.a2z2+totalImpulse3*this.a2z3; }, solve:function(){ var rvx=this.l2.x-this.l1.x+this.a2.y*this.r2z-this.a2.z*this.r2y-this.a1.y*this.r1z+this.a1.z*this.r1y; var rvy=this.l2.y-this.l1.y+this.a2.z*this.r2x-this.a2.x*this.r2z-this.a1.z*this.r1x+this.a1.x*this.r1z; var rvz=this.l2.z-this.l1.z+this.a2.x*this.r2y-this.a2.y*this.r2x-this.a1.x*this.r1y+this.a1.y*this.r1x; var rvn1=rvx*this.ax1+rvy*this.ay1+rvz*this.az1; var rvn2=rvx*this.ax2+rvy*this.ay2+rvz*this.az2; var rvn3=rvx*this.ax3+rvy*this.ay3+rvz*this.az3; var oldMotorImpulse1=this.motorImpulse1; var oldMotorImpulse2=this.motorImpulse2; var oldMotorImpulse3=this.motorImpulse3; var dMotorImpulse1=0; var dMotorImpulse2=0; var dMotorImpulse3=0; if(this.enableMotor1){ dMotorImpulse1=(rvn1-this.motorSpeed1)*this.dv00; this.motorImpulse1+=dMotorImpulse1; if(this.motorImpulse1>this.maxMotorImpulse1){ // clamp motor impulse this.motorImpulse1=this.maxMotorImpulse1; }else if(this.motorImpulse1<-this.maxMotorImpulse1){ this.motorImpulse1=-this.maxMotorImpulse1; } dMotorImpulse1=this.motorImpulse1-oldMotorImpulse1; } if(this.enableMotor2){ dMotorImpulse2=(rvn2-this.motorSpeed2)*this.dv11; this.motorImpulse2+=dMotorImpulse2; if(this.motorImpulse2>this.maxMotorImpulse2){ // clamp motor impulse this.motorImpulse2=this.maxMotorImpulse2; }else if(this.motorImpulse2<-this.maxMotorImpulse2){ this.motorImpulse2=-this.maxMotorImpulse2; } dMotorImpulse2=this.motorImpulse2-oldMotorImpulse2; } if(this.enableMotor3){ dMotorImpulse3=(rvn3-this.motorSpeed3)*this.dv22; this.motorImpulse3+=dMotorImpulse3; if(this.motorImpulse3>this.maxMotorImpulse3){ // clamp motor impulse this.motorImpulse3=this.maxMotorImpulse3; }else if(this.motorImpulse3<-this.maxMotorImpulse3){ this.motorImpulse3=-this.maxMotorImpulse3; } dMotorImpulse3=this.motorImpulse3-oldMotorImpulse3; } // apply motor impulse to relative velocity rvn1+=dMotorImpulse1*this.kv00+dMotorImpulse2*this.k01+dMotorImpulse3*this.k02; rvn2+=dMotorImpulse1*this.k10+dMotorImpulse2*this.kv11+dMotorImpulse3*this.k12; rvn3+=dMotorImpulse1*this.k20+dMotorImpulse2*this.k21+dMotorImpulse3*this.kv22; // subtract target velocity and applied impulse rvn1-=this.limitVelocity1+this.limitImpulse1*this.cfm1; rvn2-=this.limitVelocity2+this.limitImpulse2*this.cfm2; rvn3-=this.limitVelocity3+this.limitImpulse3*this.cfm3; var oldLimitImpulse1=this.limitImpulse1; var oldLimitImpulse2=this.limitImpulse2; var oldLimitImpulse3=this.limitImpulse3; var dLimitImpulse1=rvn1*this.d00+rvn2*this.d01+rvn3*this.d02; var dLimitImpulse2=rvn1*this.d10+rvn2*this.d11+rvn3*this.d12; var dLimitImpulse3=rvn1*this.d20+rvn2*this.d21+rvn3*this.d22; this.limitImpulse1+=dLimitImpulse1; this.limitImpulse2+=dLimitImpulse2; this.limitImpulse3+=dLimitImpulse3; // clamp var clampState=0; if(this.limitState1==2||this.limitImpulse1*this.limitState1<0){ dLimitImpulse1=-oldLimitImpulse1; rvn2+=dLimitImpulse1*this.k10; rvn3+=dLimitImpulse1*this.k20; clampState|=1; } if(this.limitState2==2||this.limitImpulse2*this.limitState2<0){ dLimitImpulse2=-oldLimitImpulse2; rvn1+=dLimitImpulse2*this.k01; rvn3+=dLimitImpulse2*this.k21; clampState|=2; } if(this.limitState3==2||this.limitImpulse3*this.limitState3<0){ dLimitImpulse3=-oldLimitImpulse3; rvn1+=dLimitImpulse3*this.k02; rvn2+=dLimitImpulse3*this.k12; clampState|=4; } // update un-clamped impulse // TODO: isolate division var det; switch(clampState){ case 1:// update 2 3 det=1/(this.k11*this.k22-this.k12*this.k21); dLimitImpulse2=(this.k22*rvn2+-this.k12*rvn3)*det; dLimitImpulse3=(-this.k21*rvn2+this.k11*rvn3)*det; break; case 2:// update 1 3 det=1/(this.k00*this.k22-this.k02*this.k20); dLimitImpulse1=(this.k22*rvn1+-this.k02*rvn3)*det; dLimitImpulse3=(-this.k20*rvn1+this.k00*rvn3)*det; break; case 3:// update 3 dLimitImpulse3=rvn3/this.k22; break; case 4:// update 1 2 det=1/(this.k00*this.k11-this.k01*this.k10); dLimitImpulse1=(this.k11*rvn1+-this.k01*rvn2)*det; dLimitImpulse2=(-this.k10*rvn1+this.k00*rvn2)*det; break; case 5:// update 2 dLimitImpulse2=rvn2/this.k11; break; case 6:// update 1 dLimitImpulse1=rvn1/this.k00; break; } this.limitImpulse1=oldLimitImpulse1+dLimitImpulse1; this.limitImpulse2=oldLimitImpulse2+dLimitImpulse2; this.limitImpulse3=oldLimitImpulse3+dLimitImpulse3; var dImpulse1=dMotorImpulse1+dLimitImpulse1; var dImpulse2=dMotorImpulse2+dLimitImpulse2; var dImpulse3=dMotorImpulse3+dLimitImpulse3; // apply impulse this.l1.x+=dImpulse1*this.l1x1+dImpulse2*this.l1x2+dImpulse3*this.l1x3; this.l1.y+=dImpulse1*this.l1y1+dImpulse2*this.l1y2+dImpulse3*this.l1y3; this.l1.z+=dImpulse1*this.l1z1+dImpulse2*this.l1z2+dImpulse3*this.l1z3; this.a1.x+=dImpulse1*this.a1x1+dImpulse2*this.a1x2+dImpulse3*this.a1x3; this.a1.y+=dImpulse1*this.a1y1+dImpulse2*this.a1y2+dImpulse3*this.a1y3; this.a1.z+=dImpulse1*this.a1z1+dImpulse2*this.a1z2+dImpulse3*this.a1z3; this.l2.x-=dImpulse1*this.l2x1+dImpulse2*this.l2x2+dImpulse3*this.l2x3; this.l2.y-=dImpulse1*this.l2y1+dImpulse2*this.l2y2+dImpulse3*this.l2y3; this.l2.z-=dImpulse1*this.l2z1+dImpulse2*this.l2z2+dImpulse3*this.l2z3; this.a2.x-=dImpulse1*this.a2x1+dImpulse2*this.a2x2+dImpulse3*this.a2x3; this.a2.y-=dImpulse1*this.a2y1+dImpulse2*this.a2y2+dImpulse3*this.a2y3; this.a2.z-=dImpulse1*this.a2z1+dImpulse2*this.a2z2+dImpulse3*this.a2z3; } } /** * A translational constraint for various joints. * @author saharan */ OIMO.TranslationalConstraint = function(joint,limitMotor){ this.cfm=NaN; this.m1=NaN; this.m2=NaN; this.i1e00=NaN; this.i1e01=NaN; this.i1e02=NaN; this.i1e10=NaN; this.i1e11=NaN; this.i1e12=NaN; this.i1e20=NaN; this.i1e21=NaN; this.i1e22=NaN; this.i2e00=NaN; this.i2e01=NaN; this.i2e02=NaN; this.i2e10=NaN; this.i2e11=NaN; this.i2e12=NaN; this.i2e20=NaN; this.i2e21=NaN; this.i2e22=NaN; this.motorDenom=NaN; this.invMotorDenom=NaN; this.invDenom=NaN; this.ax=NaN; this.ay=NaN; this.az=NaN; this.r1x=NaN; this.r1y=NaN; this.r1z=NaN; this.r2x=NaN; this.r2y=NaN; this.r2z=NaN; this.t1x=NaN; this.t1y=NaN; this.t1z=NaN; this.t2x=NaN; this.t2y=NaN; this.t2z=NaN; this.l1x=NaN; this.l1y=NaN; this.l1z=NaN; this.l2x=NaN; this.l2y=NaN; this.l2z=NaN; this.a1x=NaN; this.a1y=NaN; this.a1z=NaN; this.a2x=NaN; this.a2y=NaN; this.a2z=NaN; this.lowerLimit=NaN; this.upperLimit=NaN; this.limitVelocity=NaN; this.limitState=0; // -1: at lower, 0: locked, 1: at upper, 2: free this.enableMotor=false; this.motorSpeed=NaN; this.maxMotorForce=NaN; this.maxMotorImpulse=NaN; this.limitMotor=limitMotor; this.b1=joint.body1; this.b2=joint.body2; this.p1=joint.anchorPoint1; this.p2=joint.anchorPoint2; this.r1=joint.relativeAnchorPoint1; this.r2=joint.relativeAnchorPoint2; this.l1=this.b1.linearVelocity; this.l2=this.b2.linearVelocity; this.a1=this.b1.angularVelocity; this.a2=this.b2.angularVelocity; this.i1=this.b1.inverseInertia; this.i2=this.b2.inverseInertia; this.limitImpulse=0; this.motorImpulse=0; } OIMO.TranslationalConstraint.prototype = { constructor: OIMO.TranslationalConstraint, preSolve:function(timeStep,invTimeStep){ this.ax=this.limitMotor.axis.x; this.ay=this.limitMotor.axis.y; this.az=this.limitMotor.axis.z; this.lowerLimit=this.limitMotor.lowerLimit; this.upperLimit=this.limitMotor.upperLimit; this.motorSpeed=this.limitMotor.motorSpeed; this.maxMotorForce=this.limitMotor.maxMotorForce; this.enableMotor=this.maxMotorForce>0; this.m1=this.b1.inverseMass; this.m2=this.b2.inverseMass; var ti1 = this.i1.elements; var ti2 = this.i2.elements; this.i1e00=ti1[0]; this.i1e01=ti1[1]; this.i1e02=ti1[2]; this.i1e10=ti1[3]; this.i1e11=ti1[4]; this.i1e12=ti1[5]; this.i1e20=ti1[6]; this.i1e21=ti1[7]; this.i1e22=ti1[8]; this.i2e00=ti2[0]; this.i2e01=ti2[1]; this.i2e02=ti2[2]; this.i2e10=ti2[3]; this.i2e11=ti2[4]; this.i2e12=ti2[5]; this.i2e20=ti2[6]; this.i2e21=ti2[7]; this.i2e22=ti2[8]; var dx=this.p2.x-this.p1.x; var dy=this.p2.y-this.p1.y; var dz=this.p2.z-this.p1.z; var d=dx*this.ax+dy*this.ay+dz*this.az; var frequency=this.limitMotor.frequency; var enableSpring=frequency>0; var enableLimit=this.lowerLimit<=this.upperLimit; if(enableSpring&&d>20||d<-20){ enableSpring=false; } if(enableLimit){ if(this.lowerLimit==this.upperLimit){ if(this.limitState!=0){ this.limitState=0; this.limitImpulse=0; } this.limitVelocity=this.lowerLimit-d; if(!enableSpring)d=this.lowerLimit; }else if(dthis.upperLimit){ if(this.limitState!=1){ this.limitState=1; this.limitImpulse=0; } this.limitVelocity=this.upperLimit-d; if(!enableSpring)d=this.upperLimit; }else{ this.limitState=2; this.limitImpulse=0; this.limitVelocity=0; } if(!enableSpring){ if(this.limitVelocity>0.005)this.limitVelocity-=0.005; else if(this.limitVelocity<-0.005)this.limitVelocity+=0.005; else this.limitVelocity=0; } }else{ this.limitState=2; this.limitImpulse=0; } if(this.enableMotor&&(this.limitState!=0||enableSpring)){ this.maxMotorImpulse=this.maxMotorForce*timeStep; }else{ this.motorImpulse=0; this.maxMotorImpulse=0; } var rdx=d*this.ax; var rdy=d*this.ay; var rdz=d*this.az; var w1=this.m1/(this.m1+this.m2); var w2=1-w1; this.r1x=this.r1.x+rdx*w1; this.r1y=this.r1.y+rdy*w1; this.r1z=this.r1.z+rdz*w1; this.r2x=this.r2.x-rdx*w2; this.r2y=this.r2.y-rdy*w2; this.r2z=this.r2.z-rdz*w2; this.t1x=this.r1y*this.az-this.r1z*this.ay; this.t1y=this.r1z*this.ax-this.r1x*this.az; this.t1z=this.r1x*this.ay-this.r1y*this.ax; this.t2x=this.r2y*this.az-this.r2z*this.ay; this.t2y=this.r2z*this.ax-this.r2x*this.az; this.t2z=this.r2x*this.ay-this.r2y*this.ax; this.l1x=this.ax*this.m1; this.l1y=this.ay*this.m1; this.l1z=this.az*this.m1; this.l2x=this.ax*this.m2; this.l2y=this.ay*this.m2; this.l2z=this.az*this.m2; this.a1x=this.t1x*this.i1e00+this.t1y*this.i1e01+this.t1z*this.i1e02; this.a1y=this.t1x*this.i1e10+this.t1y*this.i1e11+this.t1z*this.i1e12; this.a1z=this.t1x*this.i1e20+this.t1y*this.i1e21+this.t1z*this.i1e22; this.a2x=this.t2x*this.i2e00+this.t2y*this.i2e01+this.t2z*this.i2e02; this.a2y=this.t2x*this.i2e10+this.t2y*this.i2e11+this.t2z*this.i2e12; this.a2z=this.t2x*this.i2e20+this.t2y*this.i2e21+this.t2z*this.i2e22; this.motorDenom= this.m1+this.m2+ this.ax*(this.a1y*this.r1z-this.a1z*this.r1y+this.a2y*this.r2z-this.a2z*this.r2y)+ this.ay*(this.a1z*this.r1x-this.a1x*this.r1z+this.a2z*this.r2x-this.a2x*this.r2z)+ this.az*(this.a1x*this.r1y-this.a1y*this.r1x+this.a2x*this.r2y-this.a2y*this.r2x); this.invMotorDenom=1/this.motorDenom; if(enableSpring&&this.limitState!=2){ var omega=6.2831853*frequency; var k=omega*omega*timeStep; var dmp=invTimeStep/(k+2*this.limitMotor.dampingRatio*omega); this.cfm=this.motorDenom*dmp; this.limitVelocity*=k*dmp; }else{ this.cfm=0; this.limitVelocity*=invTimeStep*0.05; } this.invDenom=1/(this.motorDenom+this.cfm); var totalImpulse=this.limitImpulse+this.motorImpulse; this.l1.x+=totalImpulse*this.l1x; this.l1.y+=totalImpulse*this.l1y; this.l1.z+=totalImpulse*this.l1z; this.a1.x+=totalImpulse*this.a1x; this.a1.y+=totalImpulse*this.a1y; this.a1.z+=totalImpulse*this.a1z; this.l2.x-=totalImpulse*this.l2x; this.l2.y-=totalImpulse*this.l2y; this.l2.z-=totalImpulse*this.l2z; this.a2.x-=totalImpulse*this.a2x; this.a2.y-=totalImpulse*this.a2y; this.a2.z-=totalImpulse*this.a2z; }, solve:function(){ var rvn= this.ax*(this.l2.x-this.l1.x)+this.ay*(this.l2.y-this.l1.y)+this.az*(this.l2.z-this.l1.z)+ this.t2x*this.a2.x-this.t1x*this.a1.x+this.t2y*this.a2.y-this.t1y*this.a1.y+this.t2z*this.a2.z-this.t1z*this.a1.z; // motor part var newMotorImpulse; if(this.enableMotor){ newMotorImpulse=(rvn-this.motorSpeed)*this.invMotorDenom; var oldMotorImpulse=this.motorImpulse; this.motorImpulse+=newMotorImpulse; if(this.motorImpulse>this.maxMotorImpulse)this.motorImpulse=this.maxMotorImpulse; else if(this.motorImpulse<-this.maxMotorImpulse)this.motorImpulse=-this.maxMotorImpulse; newMotorImpulse=this.motorImpulse-oldMotorImpulse; rvn-=newMotorImpulse*this.motorDenom; }else newMotorImpulse=0; // limit part var newLimitImpulse; if(this.limitState!=2){ newLimitImpulse=(rvn-this.limitVelocity-this.limitImpulse*this.cfm)*this.invDenom; var oldLimitImpulse=this.limitImpulse; this.limitImpulse+=newLimitImpulse; if(this.limitImpulse*this.limitState<0)this.limitImpulse=0; newLimitImpulse=this.limitImpulse-oldLimitImpulse; }else newLimitImpulse=0; var totalImpulse=newLimitImpulse+newMotorImpulse; this.l1.x+=totalImpulse*this.l1x; this.l1.y+=totalImpulse*this.l1y; this.l1.z+=totalImpulse*this.l1z; this.a1.x+=totalImpulse*this.a1x; this.a1.y+=totalImpulse*this.a1y; this.a1.z+=totalImpulse*this.a1z; this.l2.x-=totalImpulse*this.l2x; this.l2.y-=totalImpulse*this.l2y; this.l2.z-=totalImpulse*this.l2z; this.a2.x-=totalImpulse*this.a2x; this.a2.y-=totalImpulse*this.a2y; this.a2.z-=totalImpulse*this.a2z; } } /** * A contact is a pair of shapes whose axis-aligned bounding boxes are overlapping. * @author saharan */ OIMO.Contact = function(){ // The first shape. this.shape1=null; // The second shape. this.shape2=null; // The first rigid body. this.body1=null; // The second rigid body. this.body2=null; // The previous contact in the world. this.prev=null; // The next contact in the world. this.next=null; // Internal this.persisting=false; // Whether both the rigid bodies are sleeping or not. this.sleeping=false; // The collision detector between two shapes. this.detector=null; // The contact constraint of the contact. this.constraint=null; // Whether the shapes are touching or not. this.touching=false; this.b1Link=new OIMO.ContactLink(this); this.b2Link=new OIMO.ContactLink(this); this.s1Link=new OIMO.ContactLink(this); this.s2Link=new OIMO.ContactLink(this); // The contact manifold of the contact. this.manifold=new OIMO.ContactManifold(); this.buffer=[];// vector 4 this.buffer.length = 4; this.buffer[0]=new OIMO.ImpulseDataBuffer(); this.buffer[1]=new OIMO.ImpulseDataBuffer(); this.buffer[2]=new OIMO.ImpulseDataBuffer(); this.buffer[3]=new OIMO.ImpulseDataBuffer(); this.points=this.manifold.points; this.constraint=new OIMO.ContactConstraint(this.manifold); } OIMO.Contact.prototype = { constructor: OIMO.Contact, mixRestitution:function(restitution1,restitution2){ return Math.sqrt(restitution1*restitution2); }, mixFriction:function(friction1,friction2){ return Math.sqrt(friction1*friction2); }, /** * Update the contact manifold. */ updateManifold:function(){ this.constraint.restitution=this.mixRestitution(this.shape1.restitution,this.shape2.restitution); this.constraint.friction=this.mixFriction(this.shape1.friction,this.shape2.friction); var numBuffers=this.manifold.numPoints; var i = numBuffers; while(i--){ //for(var i=0;i0.04){ len=1/Math.sqrt(len); }else{ tanX=norY*norX-norZ*norZ; tanY=-norZ*norY-norX*norX; tanZ=norX*norZ+norY*norY; len=1/Math.sqrt(tanX*tanX+tanY*tanY+tanZ*tanZ); } tanX*=len; tanY*=len; tanZ*=len; var binX=norY*tanZ-norZ*tanY; var binY=norZ*tanX-norX*tanZ; var binZ=norX*tanY-norY*tanX; c.norX=norX; c.norY=norY; c.norZ=norZ; c.tanX=tanX; c.tanY=tanY; c.tanZ=tanZ; c.binX=binX; c.binY=binY; c.binZ=binZ; c.norU1X=norX*this.m1; c.norU1Y=norY*this.m1; c.norU1Z=norZ*this.m1; c.norU2X=norX*this.m2; c.norU2Y=norY*this.m2; c.norU2Z=norZ*this.m2; c.tanU1X=tanX*this.m1; c.tanU1Y=tanY*this.m1; c.tanU1Z=tanZ*this.m1; c.tanU2X=tanX*this.m2; c.tanU2Y=tanY*this.m2; c.tanU2Z=tanZ*this.m2; c.binU1X=binX*this.m1; c.binU1Y=binY*this.m1; c.binU1Z=binZ*this.m1; c.binU2X=binX*this.m2; c.binU2Y=binY*this.m2; c.binU2Z=binZ*this.m2; var norT1X=rp1Y*norZ-rp1Z*norY; var norT1Y=rp1Z*norX-rp1X*norZ; var norT1Z=rp1X*norY-rp1Y*norX; var norT2X=rp2Y*norZ-rp2Z*norY; var norT2Y=rp2Z*norX-rp2X*norZ; var norT2Z=rp2X*norY-rp2Y*norX; var tanT1X=rp1Y*tanZ-rp1Z*tanY; var tanT1Y=rp1Z*tanX-rp1X*tanZ; var tanT1Z=rp1X*tanY-rp1Y*tanX; var tanT2X=rp2Y*tanZ-rp2Z*tanY; var tanT2Y=rp2Z*tanX-rp2X*tanZ; var tanT2Z=rp2X*tanY-rp2Y*tanX; var binT1X=rp1Y*binZ-rp1Z*binY; var binT1Y=rp1Z*binX-rp1X*binZ; var binT1Z=rp1X*binY-rp1Y*binX; var binT2X=rp2Y*binZ-rp2Z*binY; var binT2Y=rp2Z*binX-rp2X*binZ; var binT2Z=rp2X*binY-rp2Y*binX; var norTU1X=norT1X*this.i1e00+norT1Y*this.i1e01+norT1Z*this.i1e02; var norTU1Y=norT1X*this.i1e10+norT1Y*this.i1e11+norT1Z*this.i1e12; var norTU1Z=norT1X*this.i1e20+norT1Y*this.i1e21+norT1Z*this.i1e22; var norTU2X=norT2X*this.i2e00+norT2Y*this.i2e01+norT2Z*this.i2e02; var norTU2Y=norT2X*this.i2e10+norT2Y*this.i2e11+norT2Z*this.i2e12; var norTU2Z=norT2X*this.i2e20+norT2Y*this.i2e21+norT2Z*this.i2e22; var tanTU1X=tanT1X*this.i1e00+tanT1Y*this.i1e01+tanT1Z*this.i1e02; var tanTU1Y=tanT1X*this.i1e10+tanT1Y*this.i1e11+tanT1Z*this.i1e12; var tanTU1Z=tanT1X*this.i1e20+tanT1Y*this.i1e21+tanT1Z*this.i1e22; var tanTU2X=tanT2X*this.i2e00+tanT2Y*this.i2e01+tanT2Z*this.i2e02; var tanTU2Y=tanT2X*this.i2e10+tanT2Y*this.i2e11+tanT2Z*this.i2e12; var tanTU2Z=tanT2X*this.i2e20+tanT2Y*this.i2e21+tanT2Z*this.i2e22; var binTU1X=binT1X*this.i1e00+binT1Y*this.i1e01+binT1Z*this.i1e02; var binTU1Y=binT1X*this.i1e10+binT1Y*this.i1e11+binT1Z*this.i1e12; var binTU1Z=binT1X*this.i1e20+binT1Y*this.i1e21+binT1Z*this.i1e22; var binTU2X=binT2X*this.i2e00+binT2Y*this.i2e01+binT2Z*this.i2e02; var binTU2Y=binT2X*this.i2e10+binT2Y*this.i2e11+binT2Z*this.i2e12; var binTU2Z=binT2X*this.i2e20+binT2Y*this.i2e21+binT2Z*this.i2e22; c.norT1X=norT1X; c.norT1Y=norT1Y; c.norT1Z=norT1Z; c.tanT1X=tanT1X; c.tanT1Y=tanT1Y; c.tanT1Z=tanT1Z; c.binT1X=binT1X; c.binT1Y=binT1Y; c.binT1Z=binT1Z; c.norT2X=norT2X; c.norT2Y=norT2Y; c.norT2Z=norT2Z; c.tanT2X=tanT2X; c.tanT2Y=tanT2Y; c.tanT2Z=tanT2Z; c.binT2X=binT2X; c.binT2Y=binT2Y; c.binT2Z=binT2Z; c.norTU1X=norTU1X; c.norTU1Y=norTU1Y; c.norTU1Z=norTU1Z; c.tanTU1X=tanTU1X; c.tanTU1Y=tanTU1Y; c.tanTU1Z=tanTU1Z; c.binTU1X=binTU1X; c.binTU1Y=binTU1Y; c.binTU1Z=binTU1Z; c.norTU2X=norTU2X; c.norTU2Y=norTU2Y; c.norTU2Z=norTU2Z; c.tanTU2X=tanTU2X; c.tanTU2Y=tanTU2Y; c.tanTU2Z=tanTU2Z; c.binTU2X=binTU2X; c.binTU2Y=binTU2Y; c.binTU2Z=binTU2Z; tmp1X=norT1X*this.i1e00+norT1Y*this.i1e01+norT1Z*this.i1e02; tmp1Y=norT1X*this.i1e10+norT1Y*this.i1e11+norT1Z*this.i1e12; tmp1Z=norT1X*this.i1e20+norT1Y*this.i1e21+norT1Z*this.i1e22; tmp2X=tmp1Y*rp1Z-tmp1Z*rp1Y; tmp2Y=tmp1Z*rp1X-tmp1X*rp1Z; tmp2Z=tmp1X*rp1Y-tmp1Y*rp1X; tmp1X=norT2X*this.i2e00+norT2Y*this.i2e01+norT2Z*this.i2e02; tmp1Y=norT2X*this.i2e10+norT2Y*this.i2e11+norT2Z*this.i2e12; tmp1Z=norT2X*this.i2e20+norT2Y*this.i2e21+norT2Z*this.i2e22; tmp2X+=tmp1Y*rp2Z-tmp1Z*rp2Y; tmp2Y+=tmp1Z*rp2X-tmp1X*rp2Z; tmp2Z+=tmp1X*rp2Y-tmp1Y*rp2X; var norDen=1/(m1m2+norX*tmp2X+norY*tmp2Y+norZ*tmp2Z); tmp1X=tanT1X*this.i1e00+tanT1Y*this.i1e01+tanT1Z*this.i1e02; tmp1Y=tanT1X*this.i1e10+tanT1Y*this.i1e11+tanT1Z*this.i1e12; tmp1Z=tanT1X*this.i1e20+tanT1Y*this.i1e21+tanT1Z*this.i1e22; tmp2X=tmp1Y*rp1Z-tmp1Z*rp1Y; tmp2Y=tmp1Z*rp1X-tmp1X*rp1Z; tmp2Z=tmp1X*rp1Y-tmp1Y*rp1X; tmp1X=tanT2X*this.i2e00+tanT2Y*this.i2e01+tanT2Z*this.i2e02; tmp1Y=tanT2X*this.i2e10+tanT2Y*this.i2e11+tanT2Z*this.i2e12; tmp1Z=tanT2X*this.i2e20+tanT2Y*this.i2e21+tanT2Z*this.i2e22; tmp2X+=tmp1Y*rp2Z-tmp1Z*rp2Y; tmp2Y+=tmp1Z*rp2X-tmp1X*rp2Z; tmp2Z+=tmp1X*rp2Y-tmp1Y*rp2X; var tanDen=1/(m1m2+tanX*tmp2X+tanY*tmp2Y+tanZ*tmp2Z); tmp1X=binT1X*this.i1e00+binT1Y*this.i1e01+binT1Z*this.i1e02; tmp1Y=binT1X*this.i1e10+binT1Y*this.i1e11+binT1Z*this.i1e12; tmp1Z=binT1X*this.i1e20+binT1Y*this.i1e21+binT1Z*this.i1e22; tmp2X=tmp1Y*rp1Z-tmp1Z*rp1Y; tmp2Y=tmp1Z*rp1X-tmp1X*rp1Z; tmp2Z=tmp1X*rp1Y-tmp1Y*rp1X; tmp1X=binT2X*this.i2e00+binT2Y*this.i2e01+binT2Z*this.i2e02; tmp1Y=binT2X*this.i2e10+binT2Y*this.i2e11+binT2Z*this.i2e12; tmp1Z=binT2X*this.i2e20+binT2Y*this.i2e21+binT2Z*this.i2e22; tmp2X+=tmp1Y*rp2Z-tmp1Z*rp2Y; tmp2Y+=tmp1Z*rp2X-tmp1X*rp2Z; tmp2Z+=tmp1X*rp2Y-tmp1Y*rp2X; var binDen=1/(m1m2+binX*tmp2X+binY*tmp2Y+binZ*tmp2Z); c.norDen=norDen; c.tanDen=tanDen; c.binDen=binDen; if(p.warmStarted){ var norImp=p.normalImpulse; this.lv1.x+=c.norU1X*norImp; this.lv1.y+=c.norU1Y*norImp; this.lv1.z+=c.norU1Z*norImp; this.av1.x+=norTU1X*norImp; this.av1.y+=norTU1Y*norImp; this.av1.z+=norTU1Z*norImp; this.lv2.x-=c.norU2X*norImp; this.lv2.y-=c.norU2Y*norImp; this.lv2.z-=c.norU2Z*norImp; this.av2.x-=norTU2X*norImp; this.av2.y-=norTU2Y*norImp; this.av2.z-=norTU2Z*norImp; c.norImp=norImp; c.tanImp=0; c.binImp=0; rvn=0; // disable bouncing }else{ c.norImp=0; c.tanImp=0; c.binImp=0; } if(rvn>-1){ rvn=0; // disable bouncing } var norTar=this.restitution*-rvn; var sepV=-(p.penetration+0.005)*invTimeStep*0.05; // allow 0.5cm error if(norTarmax*max){ len=max/Math.sqrt(len); tanImp*=len; binImp*=len; } newImp1=tanImp-oldImp1; newImp2=binImp-oldImp2; lv1x+=c.tanU1X*newImp1+c.binU1X*newImp2; lv1y+=c.tanU1Y*newImp1+c.binU1Y*newImp2; lv1z+=c.tanU1Z*newImp1+c.binU1Z*newImp2; av1x+=c.tanTU1X*newImp1+c.binTU1X*newImp2; av1y+=c.tanTU1Y*newImp1+c.binTU1Y*newImp2; av1z+=c.tanTU1Z*newImp1+c.binTU1Z*newImp2; lv2x-=c.tanU2X*newImp1+c.binU2X*newImp2; lv2y-=c.tanU2Y*newImp1+c.binU2Y*newImp2; lv2z-=c.tanU2Z*newImp1+c.binU2Z*newImp2; av2x-=c.tanTU2X*newImp1+c.binTU2X*newImp2; av2y-=c.tanTU2Y*newImp1+c.binTU2Y*newImp2; av2z-=c.tanTU2Z*newImp1+c.binTU2Z*newImp2; // restitution part rvn= (lv2x-lv1x)*c.norX+(lv2y-lv1y)*c.norY+(lv2z-lv1z)*c.norZ+ av2x*c.norT2X+av2y*c.norT2Y+av2z*c.norT2Z- av1x*c.norT1X-av1y*c.norT1Y-av1z*c.norT1Z; oldImp1=norImp; newImp1=(rvn-c.norTar)*c.norDen; norImp+=newImp1; if(norImp>0)norImp=0; newImp1=norImp-oldImp1; lv1x+=c.norU1X*newImp1; lv1y+=c.norU1Y*newImp1; lv1z+=c.norU1Z*newImp1; av1x+=c.norTU1X*newImp1; av1y+=c.norTU1Y*newImp1; av1z+=c.norTU1Z*newImp1; lv2x-=c.norU2X*newImp1; lv2y-=c.norU2Y*newImp1; lv2z-=c.norU2Z*newImp1; av2x-=c.norTU2X*newImp1; av2y-=c.norTU2Y*newImp1; av2z-=c.norTU2Z*newImp1; c.norImp=norImp; c.tanImp=tanImp; c.binImp=binImp; if(c.last)break; c=c.next; } this.lv1.x=lv1x; this.lv1.y=lv1y; this.lv1.z=lv1z; this.lv2.x=lv2x; this.lv2.y=lv2y; this.lv2.z=lv2z; this.av1.x=av1x; this.av1.y=av1y; this.av1.z=av1z; this.av2.x=av2x; this.av2.y=av2y; this.av2.z=av2z; } OIMO.ContactConstraint.prototype.postSolve = function(){ var c=this.cs; var i = this.num; while(i--){ //for(var i=0;i0; if(!right1)len=-len; len1=w1; dot1=a1x*a4x+a1y*a4y+a1z*a4z; dot2=a1x*a5x+a1y*a5y+a1z*a5z; dot3=a1x*a6x+a1y*a6y+a1z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len2=dot1*w2+dot2*h2+dot3*d2; overlap1=len-len1-len2; if(overlap1>0)return; // try axis 2 len=a2x*dx+a2y*dy+a2z*dz; right2=len>0; if(!right2)len=-len; len1=h1; dot1=a2x*a4x+a2y*a4y+a2z*a4z; dot2=a2x*a5x+a2y*a5y+a2z*a5z; dot3=a2x*a6x+a2y*a6y+a2z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len2=dot1*w2+dot2*h2+dot3*d2; overlap2=len-len1-len2; if(overlap2>0)return; // try axis 3 len=a3x*dx+a3y*dy+a3z*dz; right3=len>0; if(!right3)len=-len; len1=d1; dot1=a3x*a4x+a3y*a4y+a3z*a4z; dot2=a3x*a5x+a3y*a5y+a3z*a5z; dot3=a3x*a6x+a3y*a6y+a3z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len2=dot1*w2+dot2*h2+dot3*d2; overlap3=len-len1-len2; if(overlap3>0)return; // try axis 4 len=a4x*dx+a4y*dy+a4z*dz; right4=len>0; if(!right4)len=-len; dot1=a4x*a1x+a4y*a1y+a4z*a1z; dot2=a4x*a2x+a4y*a2y+a4z*a2z; dot3=a4x*a3x+a4y*a3y+a4z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len1=dot1*w1+dot2*h1+dot3*d1; len2=w2; overlap4=(len-len1-len2)*1.0; if(overlap4>0)return; // try axis 5 len=a5x*dx+a5y*dy+a5z*dz; right5=len>0; if(!right5)len=-len; dot1=a5x*a1x+a5y*a1y+a5z*a1z; dot2=a5x*a2x+a5y*a2y+a5z*a2z; dot3=a5x*a3x+a5y*a3y+a5z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len1=dot1*w1+dot2*h1+dot3*d1; len2=h2; overlap5=(len-len1-len2)*1.0; if(overlap5>0)return; // try axis 6 len=a6x*dx+a6y*dy+a6z*dz; right6=len>0; if(!right6)len=-len; dot1=a6x*a1x+a6y*a1y+a6z*a1z; dot2=a6x*a2x+a6y*a2y+a6z*a2z; dot3=a6x*a3x+a6y*a3y+a6z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; if(dot3<0)dot3=-dot3; len1=dot1*w1+dot2*h1+dot3*d1; len2=d2; overlap6=(len-len1-len2)*1.0; if(overlap6>0)return; // try axis 7 len=a7x*a7x+a7y*a7y+a7z*a7z; if(len>1e-5){ len=1/Math.sqrt(len); a7x*=len; a7y*=len; a7z*=len; len=a7x*dx+a7y*dy+a7z*dz; right7=len>0; if(!right7)len=-len; dot1=a7x*a2x+a7y*a2y+a7z*a2z; dot2=a7x*a3x+a7y*a3y+a7z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*h1+dot2*d1; dot1=a7x*a5x+a7y*a5y+a7z*a5z; dot2=a7x*a6x+a7y*a6y+a7z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*h2+dot2*d2; overlap7=len-len1-len2; if(overlap7>0)return; }else{ right7=false; overlap7=0; invalid7=true; } // try axis 8 len=a8x*a8x+a8y*a8y+a8z*a8z; if(len>1e-5){ len=1/Math.sqrt(len); a8x*=len; a8y*=len; a8z*=len; len=a8x*dx+a8y*dy+a8z*dz; right8=len>0; if(!right8)len=-len; dot1=a8x*a2x+a8y*a2y+a8z*a2z; dot2=a8x*a3x+a8y*a3y+a8z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*h1+dot2*d1; dot1=a8x*a4x+a8y*a4y+a8z*a4z; dot2=a8x*a6x+a8y*a6y+a8z*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*d2; overlap8=len-len1-len2; if(overlap8>0)return; }else{ right8=false; overlap8=0; invalid8=true; } // try axis 9 len=a9x*a9x+a9y*a9y+a9z*a9z; if(len>1e-5){ len=1/Math.sqrt(len); a9x*=len; a9y*=len; a9z*=len; len=a9x*dx+a9y*dy+a9z*dz; right9=len>0; if(!right9)len=-len; dot1=a9x*a2x+a9y*a2y+a9z*a2z; dot2=a9x*a3x+a9y*a3y+a9z*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*h1+dot2*d1; dot1=a9x*a4x+a9y*a4y+a9z*a4z; dot2=a9x*a5x+a9y*a5y+a9z*a5z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*h2; overlap9=len-len1-len2; if(overlap9>0)return; }else{ right9=false; overlap9=0; invalid9=true; } // try axis 10 len=aax*aax+aay*aay+aaz*aaz; if(len>1e-5){ len=1/Math.sqrt(len); aax*=len; aay*=len; aaz*=len; len=aax*dx+aay*dy+aaz*dz; righta=len>0; if(!righta)len=-len; dot1=aax*a1x+aay*a1y+aaz*a1z; dot2=aax*a3x+aay*a3y+aaz*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*d1; dot1=aax*a5x+aay*a5y+aaz*a5z; dot2=aax*a6x+aay*a6y+aaz*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*h2+dot2*d2; overlapa=len-len1-len2; if(overlapa>0)return; }else{ righta=false; overlapa=0; invalida=true; } // try axis 11 len=abx*abx+aby*aby+abz*abz; if(len>1e-5){ len=1/Math.sqrt(len); abx*=len; aby*=len; abz*=len; len=abx*dx+aby*dy+abz*dz; rightb=len>0; if(!rightb)len=-len; dot1=abx*a1x+aby*a1y+abz*a1z; dot2=abx*a3x+aby*a3y+abz*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*d1; dot1=abx*a4x+aby*a4y+abz*a4z; dot2=abx*a6x+aby*a6y+abz*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*d2; overlapb=len-len1-len2; if(overlapb>0)return; }else{ rightb=false; overlapb=0; invalidb=true; } // try axis 12 len=acx*acx+acy*acy+acz*acz; if(len>1e-5){ len=1/Math.sqrt(len); acx*=len; acy*=len; acz*=len; len=acx*dx+acy*dy+acz*dz; rightc=len>0; if(!rightc)len=-len; dot1=acx*a1x+acy*a1y+acz*a1z; dot2=acx*a3x+acy*a3y+acz*a3z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*d1; dot1=acx*a4x+acy*a4y+acz*a4z; dot2=acx*a5x+acy*a5y+acz*a5z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*h2; overlapc=len-len1-len2; if(overlapc>0)return; }else{ rightc=false; overlapc=0; invalidc=true; } // try axis 13 len=adx*adx+ady*ady+adz*adz; if(len>1e-5){ len=1/Math.sqrt(len); adx*=len; ady*=len; adz*=len; len=adx*dx+ady*dy+adz*dz; rightd=len>0; if(!rightd)len=-len; dot1=adx*a1x+ady*a1y+adz*a1z; dot2=adx*a2x+ady*a2y+adz*a2z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*h1; dot1=adx*a5x+ady*a5y+adz*a5z; dot2=adx*a6x+ady*a6y+adz*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*h2+dot2*d2; overlapd=len-len1-len2; if(overlapd>0)return; }else{ rightd=false; overlapd=0; invalidd=true; } // try axis 14 len=aex*aex+aey*aey+aez*aez; if(len>1e-5){ len=1/Math.sqrt(len); aex*=len; aey*=len; aez*=len; len=aex*dx+aey*dy+aez*dz; righte=len>0; if(!righte)len=-len; dot1=aex*a1x+aey*a1y+aez*a1z; dot2=aex*a2x+aey*a2y+aez*a2z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*h1; dot1=aex*a4x+aey*a4y+aez*a4z; dot2=aex*a6x+aey*a6y+aez*a6z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*d2; overlape=len-len1-len2; if(overlape>0)return; }else{ righte=false; overlape=0; invalide=true; } // try axis 15 len=afx*afx+afy*afy+afz*afz; if(len>1e-5){ len=1/Math.sqrt(len); afx*=len; afy*=len; afz*=len; len=afx*dx+afy*dy+afz*dz; rightf=len>0; if(!rightf)len=-len; dot1=afx*a1x+afy*a1y+afz*a1z; dot2=afx*a2x+afy*a2y+afz*a2z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len1=dot1*w1+dot2*h1; dot1=afx*a4x+afy*a4y+afz*a4z; dot2=afx*a5x+afy*a5y+afz*a5z; if(dot1<0)dot1=-dot1; if(dot2<0)dot2=-dot2; len2=dot1*w2+dot2*h2; overlapf=len-len1-len2; if(overlapf>0)return; }else{ rightf=false; overlapf=0; invalidf=true; } // boxes are overlapping var depth=overlap1; var depth2=overlap1; var minIndex=0; var right=right1; if(overlap2>depth2){ depth=overlap2; depth2=overlap2; minIndex=1; right=right2; } if(overlap3>depth2){ depth=overlap3; depth2=overlap3; minIndex=2; right=right3; } if(overlap4>depth2){ depth=overlap4; depth2=overlap4; minIndex=3; right=right4; } if(overlap5>depth2){ depth=overlap5; depth2=overlap5; minIndex=4; right=right5; } if(overlap6>depth2){ depth=overlap6; depth2=overlap6; minIndex=5; right=right6; } if(overlap7-0.01>depth2&&!invalid7){ depth=overlap7; depth2=overlap7-0.01; minIndex=6; right=right7; } if(overlap8-0.01>depth2&&!invalid8){ depth=overlap8; depth2=overlap8-0.01; minIndex=7; right=right8; } if(overlap9-0.01>depth2&&!invalid9){ depth=overlap9; depth2=overlap9-0.01; minIndex=8; right=right9; } if(overlapa-0.01>depth2&&!invalida){ depth=overlapa; depth2=overlapa-0.01; minIndex=9; right=righta; } if(overlapb-0.01>depth2&&!invalidb){ depth=overlapb; depth2=overlapb-0.01; minIndex=10; right=rightb; } if(overlapc-0.01>depth2&&!invalidc){ depth=overlapc; depth2=overlapc-0.01; minIndex=11; right=rightc; } if(overlapd-0.01>depth2&&!invalidd){ depth=overlapd; depth2=overlapd-0.01; minIndex=12; right=rightd; } if(overlape-0.01>depth2&&!invalide){ depth=overlape; depth2=overlape-0.01; minIndex=13; right=righte; } if(overlapf-0.01>depth2&&!invalidf){ depth=overlapf; minIndex=14; right=rightf; } // normal var nx=0; var ny=0; var nz=0; // edge line or face side normal var n1x=0; var n1y=0; var n1z=0; var n2x=0; var n2y=0; var n2z=0; // center of current face var cx=0; var cy=0; var cz=0; // face side var s1x=0; var s1y=0; var s1z=0; var s2x=0; var s2y=0; var s2z=0; // swap b1 b2 var swap=false; //_______________________________________ if(minIndex==0){// b1.x * b2 if(right){ cx=p1x+d1x; cy=p1y+d1y; cz=p1z+d1z; nx=a1x; ny=a1y; nz=a1z; }else{ cx=p1x-d1x; cy=p1y-d1y; cz=p1z-d1z; nx=-a1x; ny=-a1y; nz=-a1z; } s1x=d2x; s1y=d2y; s1z=d2z; n1x=-a2x; n1y=-a2y; n1z=-a2z; s2x=d3x; s2y=d3y; s2z=d3z; n2x=-a3x; n2y=-a3y; n2z=-a3z; } else if(minIndex==1){// b1.y * b2 if(right){ cx=p1x+d2x; cy=p1y+d2y; cz=p1z+d2z; nx=a2x; ny=a2y; nz=a2z; }else{ cx=p1x-d2x; cy=p1y-d2y; cz=p1z-d2z; nx=-a2x; ny=-a2y; nz=-a2z; } s1x=d1x; s1y=d1y; s1z=d1z; n1x=-a1x; n1y=-a1y; n1z=-a1z; s2x=d3x; s2y=d3y; s2z=d3z; n2x=-a3x; n2y=-a3y; n2z=-a3z; } else if(minIndex==2){// b1.z * b2 if(right){ cx=p1x+d3x; cy=p1y+d3y; cz=p1z+d3z; nx=a3x; ny=a3y; nz=a3z; }else{ cx=p1x-d3x; cy=p1y-d3y; cz=p1z-d3z; nx=-a3x; ny=-a3y; nz=-a3z; } s1x=d1x; s1y=d1y; s1z=d1z; n1x=-a1x; n1y=-a1y; n1z=-a1z; s2x=d2x; s2y=d2y; s2z=d2z; n2x=-a2x; n2y=-a2y; n2z=-a2z; } else if(minIndex==3){// b2.x * b1 swap=true; if(!right){ cx=p2x+d4x; cy=p2y+d4y; cz=p2z+d4z; nx=a4x; ny=a4y; nz=a4z; }else{ cx=p2x-d4x; cy=p2y-d4y; cz=p2z-d4z; nx=-a4x; ny=-a4y; nz=-a4z; } s1x=d5x; s1y=d5y; s1z=d5z; n1x=-a5x; n1y=-a5y; n1z=-a5z; s2x=d6x; s2y=d6y; s2z=d6z; n2x=-a6x; n2y=-a6y; n2z=-a6z; } else if(minIndex==4){// b2.y * b1 swap=true; if(!right){ cx=p2x+d5x; cy=p2y+d5y; cz=p2z+d5z; nx=a5x; ny=a5y; nz=a5z; }else{ cx=p2x-d5x; cy=p2y-d5y; cz=p2z-d5z; nx=-a5x; ny=-a5y; nz=-a5z; } s1x=d4x; s1y=d4y; s1z=d4z; n1x=-a4x; n1y=-a4y; n1z=-a4z; s2x=d6x; s2y=d6y; s2z=d6z; n2x=-a6x; n2y=-a6y; n2z=-a6z; } else if(minIndex==5){// b2.z * b1 swap=true; if(!right){ cx=p2x+d6x; cy=p2y+d6y; cz=p2z+d6z; nx=a6x; ny=a6y; nz=a6z; }else{ cx=p2x-d6x; cy=p2y-d6y; cz=p2z-d6z; nx=-a6x; ny=-a6y; nz=-a6z; } s1x=d4x; s1y=d4y; s1z=d4z; n1x=-a4x; n1y=-a4y; n1z=-a4z; s2x=d5x; s2y=d5y; s2z=d5z; n2x=-a5x; n2y=-a5y; n2z=-a5z; } else if(minIndex==6){// b1.x * b2.x nx=a7x; ny=a7y; nz=a7z; n1x=a1x; n1y=a1y; n1z=a1z; n2x=a4x; n2y=a4y; n2z=a4z; } else if(minIndex==7){// b1.x * b2.y nx=a8x; ny=a8y; nz=a8z; n1x=a1x; n1y=a1y; n1z=a1z; n2x=a5x; n2y=a5y; n2z=a5z; } else if(minIndex==8){// b1.x * b2.z nx=a9x; ny=a9y; nz=a9z; n1x=a1x; n1y=a1y; n1z=a1z; n2x=a6x; n2y=a6y; n2z=a6z; } else if(minIndex==9){// b1.y * b2.x nx=aax; ny=aay; nz=aaz; n1x=a2x; n1y=a2y; n1z=a2z; n2x=a4x; n2y=a4y; n2z=a4z } else if(minIndex==10){// b1.y * b2.y nx=abx; ny=aby; nz=abz; n1x=a2x; n1y=a2y; n1z=a2z; n2x=a5x; n2y=a5y; n2z=a5z; } else if(minIndex==11){// b1.y * b2.z nx=acx; ny=acy; nz=acz; n1x=a2x; n1y=a2y; n1z=a2z; n2x=a6x; n2y=a6y; n2z=a6z; } else if(minIndex==12){// b1.z * b2.x nx=adx; ny=ady; nz=adz; n1x=a3x; n1y=a3y; n1z=a3z; n2x=a4x; n2y=a4y; n2z=a4z; } else if(minIndex==13){// b1.z * b2.y nx=aex; ny=aey; nz=aez; n1x=a3x; n1y=a3y; n1z=a3z; n2x=a5x; n2y=a5y; n2z=a5z; } else if(minIndex==14){// b1.z * b2.z nx=afx; ny=afy; nz=afz; n1x=a3x; n1y=a3y; n1z=a3z; n2x=a6x; n2y=a6y; n2z=a6z; } //__________________________________________ var v; if(minIndex>5){ if(!right){ nx=-nx; ny=-ny; nz=-nz; } var distance; var maxDistance; var vx; var vy; var vz; var v1x; var v1y; var v1z; var v2x; var v2y; var v2z; //vertex1; v1x=V1[0]; v1y=V1[1]; v1z=V1[2]; maxDistance=nx*v1x+ny*v1y+nz*v1z; //vertex2; vx=V1[3]; vy=V1[4]; vz=V1[5]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex3; vx=V1[6]; vy=V1[7]; vz=V1[8]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex4; vx=V1[9]; vy=V1[10]; vz=V1[11]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex5; vx=V1[12]; vy=V1[13]; vz=V1[14]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex6; vx=V1[15]; vy=V1[16]; vz=V1[17]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex7; vx=V1[18]; vy=V1[19]; vz=V1[20]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex8; vx=V1[21]; vy=V1[22]; vz=V1[23]; distance=nx*vx+ny*vy+nz*vz; if(distance>maxDistance){ maxDistance=distance; v1x=vx; v1y=vy; v1z=vz; } //vertex1; v2x=V2[0]; v2y=V2[1]; v2z=V2[2]; maxDistance=nx*v2x+ny*v2y+nz*v2z; //vertex2; vx=V2[3]; vy=V2[4]; vz=V2[5]; distance=nx*vx+ny*vy+nz*vz; if(distance0){ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices2[index]=x2; this.clipVertices2[index+1]=y2; this.clipVertices2[index+2]=z2; }else{ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices2[index]=x1+(x2-x1)*t; this.clipVertices2[index+1]=y1+(y2-y1)*t; this.clipVertices2[index+2]=z1+(z2-z1)*t; } }else{ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices2[index]=x1+(x2-x1)*t; this.clipVertices2[index+1]=y1+(y2-y1)*t; this.clipVertices2[index+2]=z1+(z2-z1)*t; index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices2[index]=x2; this.clipVertices2[index+1]=y2; this.clipVertices2[index+2]=z2; } } x1=x2; y1=y2; z1=z2; dot1=dot2; } numClipVertices=numAddedClipVertices; if(numClipVertices==0)return; numAddedClipVertices=0; index=(numClipVertices-1)*3; x1=this.clipVertices2[index]; y1=this.clipVertices2[index+1]; z1=this.clipVertices2[index+2]; dot1=(x1-cx-s2x)*n2x+(y1-cy-s2y)*n2y+(z1-cz-s2z)*n2z; //i = numClipVertices; //while(i--){ for(i=0;i0){ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices1[index]=x2; this.clipVertices1[index+1]=y2; this.clipVertices1[index+2]=z2; }else{ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices1[index]=x1+(x2-x1)*t; this.clipVertices1[index+1]=y1+(y2-y1)*t; this.clipVertices1[index+2]=z1+(z2-z1)*t; } }else{ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices1[index]=x1+(x2-x1)*t; this.clipVertices1[index+1]=y1+(y2-y1)*t; this.clipVertices1[index+2]=z1+(z2-z1)*t; index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices1[index]=x2; this.clipVertices1[index+1]=y2; this.clipVertices1[index+2]=z2; } } x1=x2; y1=y2; z1=z2; dot1=dot2; } numClipVertices=numAddedClipVertices; if(numClipVertices==0)return; numAddedClipVertices=0; index=(numClipVertices-1)*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot1=(x1-cx+s1x)*-n1x+(y1-cy+s1y)*-n1y+(z1-cz+s1z)*-n1z; //i = numClipVertices; //while(i--){ for(i=0;i0){ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices2[index]=x2; this.clipVertices2[index+1]=y2; this.clipVertices2[index+2]=z2; }else{ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices2[index]=x1+(x2-x1)*t; this.clipVertices2[index+1]=y1+(y2-y1)*t; this.clipVertices2[index+2]=z1+(z2-z1)*t; } }else{ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices2[index]=x1+(x2-x1)*t; this.clipVertices2[index+1]=y1+(y2-y1)*t; this.clipVertices2[index+2]=z1+(z2-z1)*t; index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices2[index]=x2; this.clipVertices2[index+1]=y2; this.clipVertices2[index+2]=z2; } } x1=x2; y1=y2; z1=z2; dot1=dot2; } numClipVertices=numAddedClipVertices; if(numClipVertices==0)return; numAddedClipVertices=0; index=(numClipVertices-1)*3; x1=this.clipVertices2[index]; y1=this.clipVertices2[index+1]; z1=this.clipVertices2[index+2]; dot1=(x1-cx+s2x)*-n2x+(y1-cy+s2y)*-n2y+(z1-cz+s2z)*-n2z; //i = numClipVertices; //while(i--){ for(i=0;i0){ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices1[index]=x2; this.clipVertices1[index+1]=y2; this.clipVertices1[index+2]=z2; }else{ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices1[index]=x1+(x2-x1)*t; this.clipVertices1[index+1]=y1+(y2-y1)*t; this.clipVertices1[index+2]=z1+(z2-z1)*t; } }else{ if(dot2>0){ index=numAddedClipVertices*3; numAddedClipVertices++; t=dot1/(dot1-dot2); this.clipVertices1[index]=x1+(x2-x1)*t; this.clipVertices1[index+1]=y1+(y2-y1)*t; this.clipVertices1[index+2]=z1+(z2-z1)*t; index=numAddedClipVertices*3; numAddedClipVertices++; this.clipVertices1[index]=x2; this.clipVertices1[index+1]=y2; this.clipVertices1[index+2]=z2; } } x1=x2; y1=y2; z1=z2; dot1=dot2; } numClipVertices=numAddedClipVertices; if(swap){ var tb=b1; b1=b2; b2=tb; } if(numClipVertices==0)return; var flipped=b1!=shape1; if(numClipVertices>4){ x1=(q1x+q2x+q3x+q4x)*0.25; y1=(q1y+q2y+q3y+q4y)*0.25; z1=(q1z+q2z+q3z+q4z)*0.25; n1x=q1x-x1; n1y=q1y-y1; n1z=q1z-z1; n2x=q2x-x1; n2y=q2y-y1; n2z=q2z-z1; var index1=0; var index2=0; var index3=0; var index4=0; var maxDot=-this.INF; minDot=this.INF; //i = numClipVertices; //while(i--){ for(i=0;imaxDot){ maxDot=dot; index3=i; } } this.used[index1]=true; this.used[index3]=true; maxDot=-this.INF; minDot=this.INF; //i = numClipVertices; //while(i--){ for(i=0;imaxDot){ maxDot=dot; index4=i; } } index=index1*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz; if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped); index=index2*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz; if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped); index=index3*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz; if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped); index=index4*3; x1=this.clipVertices1[index]; y1=this.clipVertices1[index+1]; z1=this.clipVertices1[index+2]; dot=(x1-cx)*nx+(y1-cy)*ny+(z1-cz)*nz; if(dot<0) manifold.addPoint(x1,y1,z1,nx,ny,nz,dot,flipped); }else{ //i = numClipVertices; //while(i--){ for(i=0;ihw){ sx=hw; }else if(sx<-hw){ sx=-hw; }else{ overlap=1; } if(sy>hh){ sy=hh; }else if(sy<-hh){ sy=-hh; }else{ overlap|=2; } if(sz>hd){ sz=hd; }else if(sz<-hd){ sz=-hd; }else{ overlap|=4; } if(overlap==7){ // center of sphere is in the box if(sx<0){ dx=hw+sx; }else{ dx=hw-sx; } if(sy<0){ dy=hh+sy; }else{ dy=hh-sy; } if(sz<0){ dz=hd+sz; }else{ dz=hd-sz; } if(dx0&&len0&&lenaabb2.maxX) ? aabb1.maxX : aabb2.maxX; this.minY = (aabb1.minYaabb2.maxY) ? aabb1.maxY : aabb2.maxY; this.minZ = (aabb1.minZaabb2.maxZ) ? aabb1.maxZ : aabb2.maxZ; /* var margin=0; this.minX-=margin; this.minY-=margin; this.minZ-=margin; this.maxX+=margin; this.maxY+=margin; this.maxZ+=margin; */ }, /** * Get the surface area. * @return */ surfaceArea:function(){ var h=this.maxY-this.minY; var d=this.maxZ-this.minZ; return 2*((this.maxX-this.minX)*(h+d)+h*d); }, /** * Get whether the AABB intersects with the point or not. * @param x * @param y * @param z * @return */ intersectsWithPoint:function(x,y,z){ return x>=this.minX&&x<=this.maxX&&y>=this.minY&&y<=this.maxY&&z>=this.minZ&&z<=this.maxZ; } } /** * A proxy is used for broad-phase collecting pairs that can be colliding. */ OIMO.Proxy = function(shape){ // The parent shape. this.shape=shape; // The axis-aligned bounding box. this.aabb=shape.aabb; }; OIMO.Proxy.prototype = { constructor: OIMO.Proxy, /** * Update the proxy. */ update:function(){ throw new Error("Inheritance error."); } } /** * A basic implementation of proxies. * @author saharan */ OIMO.BasicProxy = function(shape){ OIMO.Proxy.call( this, shape ); } OIMO.BasicProxy.prototype = Object.create( OIMO.Proxy.prototype ); OIMO.BasicProxy.prototype.update = function () { } /** * The broad-phase is used for collecting all possible pairs for collision. */ OIMO.BroadPhase = function(){ this.types = 0x0; this.numPairChecks=0; this.numPairs=0; this.bufferSize=256; this.pairs=[];// vector this.pairs.length = this.bufferSize; var i = this.bufferSize; while(i--){ this.pairs[i] = new OIMO.Pair(); } } OIMO.BroadPhase.prototype = { constructor: OIMO.BroadPhase, /** * Create a new proxy. * @param shape * @return */ createProxy:function(shape){ throw new Error("Inheritance error."); }, /** * Add the proxy into the broad-phase. * @param proxy */ addProxy:function(proxy){ throw new Error("Inheritance error."); }, /** * Remove the proxy from the broad-phase. * @param proxy */ removeProxy:function(proxy){ throw new Error("Inheritance error."); }, /** * Returns whether the pair is available or not. * @param s1 * @param s2 * @return */ isAvailablePair:function(s1,s2){ var b1=s1.parent; var b2=s2.parent; if( b1==b2 || // same parents (!b1.isDynamic&&!b2.isDynamic) || // static or kinematic object (s1.belongsTo&s2.collidesWith)==0 || (s2.belongsTo&s1.collidesWith)==0 // collision filtering ){ return false; } var js; if(b1.numJoints0){ var pair=this.pairs[--this.numPairs]; pair.shape1=null; pair.shape2=null; } this.numPairChecks=0; this.collectPairs(); }, collectPairs:function(){ throw new Error("Inheritance error."); }, addPair:function(s1,s2){ if(this.numPairs==this.bufferSize){ // expand pair buffer var newBufferSize=this.bufferSize<<1; //var newBufferSize=this.bufferSize*2; var newPairs=[];// vector newPairs.length = this.bufferSize; //var i = this.bufferSize; //var j; //while(i--){ for(var i=0, j=this.bufferSize;i= j){ for(i=this.bufferSize, j=newBufferSize;i>1; //this.numPairChecks=this.numProxies*(this.numProxies-1)*0.5; var i = this.numProxies; while(i--){ //for(var i=0, l=this.numProxies;ib2.maxX|| b1.maxYb2.maxY|| b1.maxZb2.maxZ|| !this.isAvailablePair(s1,s2) ){ continue; } this.addPair(s1,s2); }} } } /** * A pair of shapes that may collide. * @author saharan */ OIMO.Pair = function(){ // The first shape. this.shape1=null; // The second shape. this.shape2=null; }; /** * A projection axis for sweep and prune broad-phase. * @author saharan */ OIMO.SAPAxis = function(){ this.numElements=0; this.bufferSize=256; this.elements=[]; this.elements.length = this.bufferSize; this.stack=new OIMO_ARRAY_TYPE(64); }; OIMO.SAPAxis.prototype = { constructor: OIMO.SAPAxis, addElements:function(min,max){ if(this.numElements+2>=this.bufferSize){ //this.bufferSize<<=1; this.bufferSize*=2; var newElements=[]; var i = this.numElements; while(i--){ //for(var i=0, l=this.numElements; i>threshold)!=0)threshold++; threshold=threshold*this.numElements>>2; count=0; var giveup=false; var elements=this.elements; for(var i=1, l=this.numElements; ipivot){ var j=i; do{ elements[j]=tmp2; if(--j==0)break; tmp2=elements[j-1]; }while(tmp2.value>pivot); elements[j]=tmp; count+=i-j; if(count>threshold){ giveup=true; // stop and use quick sort break; } } } if(!giveup)return; count=2;var stack=this.stack; stack[0]=0; stack[1]=this.numElements-1; while(count>0){ var right=stack[--count]; var left=stack[--count]; var diff=right-left; if(diff>16){ // quick sort //var mid=left+(diff>>1); var mid=left+(Math.floor(diff*0.5)); tmp=elements[mid]; elements[mid]=elements[right]; elements[right]=tmp; pivot=tmp.value; i=left-1; j=right; while(true){ var ei; var ej; do{ ei=elements[++i]; }while(ei.value=j)break; elements[i]=ej; elements[j]=ei; } elements[right]=elements[i]; elements[i]=tmp; if(i-left>right-i){ stack[count++]=left; stack[count++]=i-1; stack[count++]=i+1; stack[count++]=right; }else{ stack[count++]=i+1; stack[count++]=right; stack[count++]=left; stack[count++]=i-1; } }else{ for(i=left+1;i<=right;i++){ tmp=elements[i]; pivot=tmp.value; tmp2=elements[i-1]; if(tmp2.value>pivot){ j=i; do{ elements[j]=tmp2; if(--j==0)break; tmp2=elements[j-1]; }while(tmp2.value>pivot); elements[j]=tmp; } } } } }, calculateTestCount:function(){ var num=1; var sum=0; for(var i=1, l=this.numElements; ie2.max1.value||max1e2.max2.value||max2e2.max1.value||max1e2.max2.value||max20){ newParent=this.freeNodes[--this.numFreeNodes]; }else{ newParent=new OIMO.DBVTNode(); } newParent.parent=oldParent; newParent.child1=leaf; newParent.child2=sibling; newParent.aabb.combine(leaf.aabb,sibling.aabb); newParent.height=sibling.height+1; sibling.parent=newParent; leaf.parent=newParent; if(sibling==this.root){ // replace root this.root=newParent; }else{ // replace child if(oldParent.child1==sibling){ oldParent.child1=newParent; }else{ oldParent.child2=newParent; } } // update whole tree do{ newParent=this.balance(newParent); this.fix(newParent); newParent=newParent.parent; }while(newParent!=null); }, getBalance:function(node){ if(node.proxy!=null)return 0; return node.child1.height-node.child2.height; }, print:function(node,indent,text){ var hasChild=node.proxy==null; if(hasChild)text=this.print(node.child1,indent+1,text); for(var i=indent*2;i>=0;i--){ text+=" "; } text+=(hasChild?this.getBalance(node):"["+node.proxy.aabb.minX+"]")+"\n"; if(hasChild)text=this.print(node.child2,indent+1,text); return text; }, /** * Delete a leaf from the tree. * @param node */ deleteLeaf:function(leaf){ if(leaf==this.root){ this.root=null; return; } var parent=leaf.parent; var sibling; if(parent.child1==leaf){ sibling=parent.child2; }else{ sibling=parent.child1; } if(parent==this.root){ this.root=sibling; sibling.parent=null; return; } var grandParent=parent.parent; sibling.parent=grandParent; if(grandParent.child1==parent){ grandParent.child1=sibling; }else{ grandParent.child2=sibling; } if(this.numFreeNodes<16384){ this.freeNodes[this.numFreeNodes++]=parent; } do{ grandParent=this.balance(grandParent); this.fix(grandParent); grandParent=grandParent.parent; }while(grandParent!=null); }, balance:function(node){ var nh=node.height; if(nh<2){ return node; } var p=node.parent; var l=node.child1; var r=node.child2; var lh=l.height; var rh=r.height; var balance=lh-rh; var t;// for bit operation // [ N ] // / \ // [ L ] [ R ] // / \ / \ // [L-L] [L-R] [R-L] [R-R] // Is the tree balanced? if(balance>1){ var ll=l.child1; var lr=l.child2; var llh=ll.height; var lrh=lr.height; // Is L-L higher than L-R? if(llh>lrh){ // set N to L-R l.child2=node; node.parent=l; // [ L ] // / \ // [L-L] [ N ] // / \ / \ // [...] [...] [ L ] [ R ] // set L-R node.child1=lr; lr.parent=node; // [ L ] // / \ // [L-L] [ N ] // / \ / \ // [...] [...] [L-R] [ R ] // fix bounds and heights node.aabb.combine(lr.aabb,r.aabb); t=lrh-rh; node.height=lrh-(t&t>>31)+1; l.aabb.combine(ll.aabb,node.aabb); t=llh-nh; l.height=llh-(t&t>>31)+1; }else{ // set N to L-L l.child1=node; node.parent=l; // [ L ] // / \ // [ N ] [L-R] // / \ / \ // [ L ] [ R ] [...] [...] // set L-L node.child1=ll; ll.parent=node; // [ L ] // / \ // [ N ] [L-R] // / \ / \ // [L-L] [ R ] [...] [...] // fix bounds and heights node.aabb.combine(ll.aabb,r.aabb); t=llh-rh; node.height=llh-(t&t>>31)+1; l.aabb.combine(node.aabb,lr.aabb); t=nh-lrh; l.height=nh-(t&t>>31)+1; } // set new parent of L if(p!=null){ if(p.child1==node){ p.child1=l; }else{ p.child2=l; } }else{ this.root=l; } l.parent=p; return l; }else if(balance<-1){ var rl=r.child1; var rr=r.child2; var rlh=rl.height; var rrh=rr.height; // Is R-L higher than R-R? if(rlh>rrh){ // set N to R-R r.child2=node; node.parent=r; // [ R ] // / \ // [R-L] [ N ] // / \ / \ // [...] [...] [ L ] [ R ] // set R-R node.child2=rr; rr.parent=node; // [ R ] // / \ // [R-L] [ N ] // / \ / \ // [...] [...] [ L ] [R-R] // fix bounds and heights node.aabb.combine(l.aabb,rr.aabb); t=lh-rrh; node.height=lh-(t&t>>31)+1; r.aabb.combine(rl.aabb,node.aabb); t=rlh-nh; r.height=rlh-(t&t>>31)+1; }else{ // set N to R-L r.child1=node; node.parent=r; // [ R ] // / \ // [ N ] [R-R] // / \ / \ // [ L ] [ R ] [...] [...] // set R-L node.child2=rl; rl.parent=node; // [ R ] // / \ // [ N ] [R-R] // / \ / \ // [ L ] [R-L] [...] [...] // fix bounds and heights node.aabb.combine(l.aabb,rl.aabb); t=lh-rlh; node.height=lh-(t&t>>31)+1; r.aabb.combine(node.aabb,rr.aabb); t=nh-rrh; r.height=nh-(t&t>>31)+1; } // set new parent of R if(p!=null){ if(p.child1==node){ p.child1=r; }else{ p.child2=r; } }else{ this.root=r; } r.parent=p; return r; } return node; }, fix:function(node){ var c1=node.child1; var c2=node.child2; node.aabb.combine(c1.aabb,c2.aabb); var h1=c1.height; var h2=c2.height; if(h1leafB.maxX|| trueB.minYleafB.maxY|| trueB.minZleafB.maxZ ){// the leaf needs correcting var margin=0.1; this.tree.deleteLeaf(leaf); leafB.minX=trueB.minX-margin; leafB.maxX=trueB.maxX+margin; leafB.minY=trueB.minY-margin; leafB.maxY=trueB.maxY+margin; leafB.minZ=trueB.minZ-margin; leafB.maxZ=trueB.maxZ+margin; this.tree.insertLeaf(leaf); this.collide(leaf,this.tree.root); } } } OIMO.DBVTBroadPhase.prototype.collide = function (node1,node2) { var stackCount=2; this.stack[0]=node1; this.stack[1]=node2; while(stackCount>0){ var n1=this.stack[--stackCount]; var n2=this.stack[--stackCount]; var l1=n1.proxy!=null; var l2=n2.proxy!=null; this.numPairChecks++; if(l1&&l2){ var s1=n1.proxy.shape; var s2=n2.proxy.shape; var b1=s1.aabb; var b2=s2.aabb; if( s1==s2|| b1.maxXb2.maxX|| b1.maxYb2.maxY|| b1.maxZb2.maxZ|| !this.isAvailablePair(s1,s2) ){ continue; } this.addPair(s1,s2); }else{ b1=n1.aabb; b2=n2.aabb; if( b1.maxXb2.maxX|| b1.maxYb2.maxY|| b1.maxZb2.maxZ ){ continue; } if(stackCount+4>=this.maxStack){// expand the stack //this.maxStack<<=1; this.maxStack*=2; var newStack=[];// vector newStack.length = this.maxStack; for(var i=0;in2.aabb.surfaceArea())){ this.stack[stackCount++]=n1.child1; this.stack[stackCount++]=n2; this.stack[stackCount++]=n1.child2; this.stack[stackCount++]=n2; }else{ this.stack[stackCount++]=n1; this.stack[stackCount++]=n2.child1; this.stack[stackCount++]=n1; this.stack[stackCount++]=n2.child2; } } } } /** * A node of the dynamic bounding volume tree. * @author saharan */ OIMO.DBVTNode = function(){ // The first child node of this node. this.child1=null; // The second child node of this node. this.child2=null; // The parent node of this tree. this.parent=null; // The proxy of this node. This has no value if this node is not leaf. this.proxy=null; // The maximum distance from leaf nodes. this.height=0; // The AABB of this node. this.aabb=new OIMO.AABB(); } /** * A proxy for dynamic bounding volume tree broad-phase. * @author saharan */ OIMO.DBVTProxy = function(shape){ OIMO.Proxy.call( this, shape); // The leaf of the proxy. this.leaf = new OIMO.DBVTNode(); this.leaf.proxy = this; } OIMO.DBVTProxy.prototype = Object.create( OIMO.Proxy.prototype ); OIMO.DBVTProxy.prototype.update = function () { }