mirror of
https://github.com/MapMakersAndProgrammers/alternativaphysics-archive.git
synced 2025-10-26 01:49:13 -07:00
329 lines
9.1 KiB
Plaintext
329 lines
9.1 KiB
Plaintext
package alternativa.physics.rigid {
|
|
import __AS3__.vec.Vector;
|
|
|
|
import alternativa.physics.altphysics;
|
|
import alternativa.physics.collision.primitives.CollisionPrimitive;
|
|
import alternativa.physics.types.Matrix3;
|
|
import alternativa.physics.types.Matrix4;
|
|
import alternativa.physics.types.Quaternion;
|
|
import alternativa.physics.types.Vector3;
|
|
|
|
use namespace altphysics;
|
|
|
|
/**
|
|
*
|
|
*/
|
|
public class Body {
|
|
|
|
public static var linDamping:Number = 0.997;
|
|
public static var rotDamping:Number = 0.997;
|
|
|
|
public var name:String;
|
|
public var movable:Boolean = true;
|
|
|
|
altphysics var id:int;
|
|
// Мир, в котором находится тело
|
|
altphysics var world:RigidWorld;
|
|
// Текущее и предыдущее состояние тела. Промежуточное состояние вычисляется линейной интерполяцией.
|
|
altphysics var state:BodyState = new BodyState();
|
|
altphysics var prevState:BodyState = new BodyState();
|
|
// Линейное и угловое ускорение тела на текущем шаге симуляции
|
|
altphysics var accel:Vector3 = new Vector3();
|
|
altphysics var angleAccel:Vector3 = new Vector3();
|
|
// Материал тела
|
|
altphysics var material:BodyMaterial = new BodyMaterial();
|
|
|
|
altphysics var invMass:Number = 1;
|
|
altphysics var invInertia:Matrix3 = new Matrix3();
|
|
altphysics var invInertiaWorld:Matrix3 = new Matrix3();
|
|
altphysics var baseMatrix:Matrix3 = new Matrix3();
|
|
|
|
altphysics const MAX_CONTACTS:int = 20;
|
|
altphysics var contacts:Vector.<Contact> = new Vector.<Contact>(MAX_CONTACTS);
|
|
altphysics var contactsNum:int;
|
|
|
|
altphysics var collisionPrimitives:Vector.<CollisionPrimitive>;
|
|
altphysics var collisionPrimitivesNum:int;
|
|
|
|
// Аккумулятор сил
|
|
altphysics var forceAccum:Vector3 = new Vector3();
|
|
// Аккумулятор моментов
|
|
altphysics var torqueAccum:Vector3 = new Vector3();
|
|
|
|
// Внутренние переменные для избежания создания экземпляров
|
|
private static var _r:Vector3 = new Vector3();
|
|
private static var _f:Vector3 = new Vector3();
|
|
|
|
/**
|
|
*
|
|
* @param invMass
|
|
* @param invInertia
|
|
*/
|
|
public function Body(invMass:Number, invInertia:Matrix3) {
|
|
this.invMass = invMass;
|
|
this.invInertia.copy(invInertia);
|
|
}
|
|
|
|
/**
|
|
* @param prim
|
|
*/
|
|
public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void {
|
|
if (primitive == null) throw new ArgumentError("Primitive cannot be null");
|
|
if (collisionPrimitives == null) collisionPrimitives = new Vector.<CollisionPrimitive>();
|
|
collisionPrimitives[collisionPrimitivesNum++] = primitive;
|
|
primitive.setBody(this, localTransform);
|
|
}
|
|
|
|
/**
|
|
* @param t
|
|
* @param result
|
|
*/
|
|
public function interpolate(t:Number, pos:Vector3, orientation:Quaternion):void {
|
|
var t1:Number = 1 - t;
|
|
pos.x = prevState.pos.x*t1 + state.pos.x*t;
|
|
pos.y = prevState.pos.y*t1 + state.pos.y*t;
|
|
pos.z = prevState.pos.z*t1 + state.pos.z*t;
|
|
orientation.w = prevState.orientation.w*t1 + state.orientation.w*t;
|
|
orientation.x = prevState.orientation.x*t1 + state.orientation.x*t;
|
|
orientation.y = prevState.orientation.y*t1 + state.orientation.y*t;
|
|
orientation.z = prevState.orientation.z*t1 + state.orientation.z*t;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param pos
|
|
*/
|
|
public function setPosition(pos:Vector3):void {
|
|
state.pos.vCopy(pos);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param x
|
|
* @param y
|
|
* @param z
|
|
*/
|
|
public function setPositionXYZ(x:Number, y:Number, z:Number):void {
|
|
state.pos.vReset(x, y, z);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param vel
|
|
*/
|
|
public function setVelocity(vel:Vector3):void {
|
|
state.velocity.vCopy(vel);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param x
|
|
* @param y
|
|
* @param z
|
|
*/
|
|
public function setVelocityXYZ(x:Number, y:Number, z:Number):void {
|
|
state.velocity.vReset(x, y, z);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param rot
|
|
*/
|
|
public function setRotation(rot:Vector3):void {
|
|
state.rotation.vCopy(rot);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param x
|
|
* @param y
|
|
* @param z
|
|
*/
|
|
public function setRotationXYZ(x:Number, y:Number, z:Number):void {
|
|
state.rotation.vReset(x, y, z);
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param q
|
|
*/
|
|
public function setOrientation(q:Quaternion):void {
|
|
state.orientation.copy(q);
|
|
}
|
|
|
|
/**
|
|
* @param r
|
|
* @param dir
|
|
* @param magnitude
|
|
*/
|
|
public function applyRelPosWorldImpulse(r:Vector3, dir:Vector3, magnitude:Number):void {
|
|
var d:Number = magnitude*invMass;
|
|
// Линейная часть
|
|
state.velocity.x += d*dir.x;
|
|
state.velocity.y += d*dir.y;
|
|
state.velocity.z += d*dir.z;
|
|
|
|
// Вращательная часть
|
|
var rx:Number = r.x;
|
|
var ry:Number = r.y;
|
|
var rz:Number = r.z;
|
|
_r.x = (ry*dir.z - rz*dir.y)*magnitude;
|
|
_r.y = (rz*dir.x - rx*dir.z)*magnitude;
|
|
_r.z = (rx*dir.y - ry*dir.x)*magnitude;
|
|
_r.vTransformBy3(invInertiaWorld);
|
|
state.rotation.x += _r.x;
|
|
state.rotation.y += _r.y;
|
|
state.rotation.z += _r.z;
|
|
}
|
|
|
|
/**
|
|
* @param f
|
|
*/
|
|
public function addForce(f:Vector3):void {
|
|
forceAccum.vAdd(f);
|
|
}
|
|
|
|
/**
|
|
* @param pos
|
|
* @param f
|
|
*/
|
|
public function addWorldForce(pos:Vector3, force:Vector3):void {
|
|
forceAccum.vAdd(force);
|
|
torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(force));
|
|
}
|
|
|
|
/**
|
|
* @param pos
|
|
* @param f
|
|
*/
|
|
public function addWorldForceScaled(pos:Vector3, force:Vector3, scale:Number):void {
|
|
_f.x = scale*force.x, _f.y = scale*force.y, _f.z = scale*force.z;
|
|
forceAccum.vAdd(_f);
|
|
torqueAccum.vAdd(_r.vDiff(pos, state.pos).vCross(_f));
|
|
}
|
|
|
|
/**
|
|
* @param pos
|
|
* @param f
|
|
*/
|
|
public function addLocalForce(pos:Vector3, force:Vector3):void {
|
|
// Трансформируем точку приложения в мировую систему координат
|
|
baseMatrix.transformVector(pos, _r);
|
|
// Трансформируем вектор силы в мировую систему
|
|
baseMatrix.transformVector(force, _f);
|
|
// Добавляем силу и момент
|
|
forceAccum.vAdd(_f);
|
|
torqueAccum.vAdd(_r.vCross(_f));
|
|
}
|
|
|
|
/**
|
|
*
|
|
* @param localPos
|
|
* @param worldForce
|
|
*/
|
|
public function addWorldForceAtLocalPoint(localPos:Vector3, worldForce:Vector3):void {
|
|
// Трансформируем точку приложения в мировую систему координат
|
|
baseMatrix.transformVector(localPos, _r);
|
|
// Добавляем силу и момент
|
|
forceAccum.vAdd(worldForce);
|
|
torqueAccum.vAdd(_r.vCross(worldForce));
|
|
}
|
|
|
|
/**
|
|
* @param dt
|
|
*/
|
|
public function addExternalForces(dt:Number):void {
|
|
}
|
|
|
|
/**
|
|
* @param t
|
|
*/
|
|
public function addTorque(t:Vector3):void {
|
|
torqueAccum.vAdd(t);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
altphysics function clearAccumulators():void {
|
|
forceAccum.vReset();
|
|
torqueAccum.vReset();
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
altphysics function calcAccelerations():void {
|
|
accel.x = forceAccum.x*invMass;
|
|
accel.y = forceAccum.y*invMass;
|
|
accel.z = forceAccum.z*invMass;
|
|
invInertiaWorld.transformVector(torqueAccum, angleAccel);
|
|
}
|
|
|
|
/**
|
|
* Вычисляет производные данные.
|
|
*/
|
|
altphysics function calcDerivedData():void {
|
|
// Вычисление базисной матрицы и обратного тензора инерции в мировых координатах
|
|
state.orientation.toMatrix3(baseMatrix);
|
|
invInertiaWorld.copy(invInertia).append(baseMatrix).prepend(baseMatrix.transpose());
|
|
baseMatrix.transpose();
|
|
if (collisionPrimitives != null) {
|
|
for (var i:int = 0; i < collisionPrimitivesNum; i++) {
|
|
var primitive:CollisionPrimitive = collisionPrimitives[i];
|
|
primitive.transform.setFromMatrix3(baseMatrix, state.pos);
|
|
if (primitive.localTransform != null) primitive.transform.prepend(primitive.localTransform);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
altphysics function saveState():void {
|
|
prevState.copy(state);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
altphysics function restoreState():void {
|
|
state.copy(prevState);
|
|
}
|
|
|
|
/**
|
|
* @param dt
|
|
*/
|
|
altphysics function integrateVelocity(dt:Number):void {
|
|
// v = v + a*t
|
|
state.velocity.x += accel.x*dt;
|
|
state.velocity.y += accel.y*dt;
|
|
state.velocity.z += accel.z*dt;
|
|
// rot = rot + eps*t
|
|
state.rotation.x += angleAccel.x*dt;
|
|
state.rotation.y += angleAccel.y*dt;
|
|
state.rotation.z += angleAccel.z*dt;
|
|
|
|
state.velocity.x *= linDamping;
|
|
state.velocity.y *= linDamping;
|
|
state.velocity.z *= linDamping;
|
|
|
|
state.rotation.x *= rotDamping;
|
|
state.rotation.y *= rotDamping;
|
|
state.rotation.z *= rotDamping;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
altphysics function integratePosition(dt:Number):void {
|
|
// pos = pos + v*t
|
|
state.pos.x += state.velocity.x*dt;
|
|
state.pos.y += state.velocity.y*dt;
|
|
state.pos.z += state.velocity.z*dt;
|
|
// q = q + 0.5*rot*q
|
|
state.orientation.addScaledVector(state.rotation, dt);
|
|
}
|
|
|
|
}
|
|
} |