mirror of
https://github.com/MapMakersAndProgrammers/alternativaphysics-archive.git
synced 2025-10-26 09:59:12 -07:00
405 lines
12 KiB
Plaintext
405 lines
12 KiB
Plaintext
package alternativa.physics {
|
||
|
||
import alternativa.math.Matrix3;
|
||
import alternativa.math.Matrix4;
|
||
import alternativa.math.Quaternion;
|
||
import alternativa.math.Vector3;
|
||
import alternativa.physics.collision.CollisionPrimitive;
|
||
import alternativa.physics.collision.IBodyCollisionPredicate;
|
||
import alternativa.physics.collision.types.BoundBox;
|
||
|
||
use namespace altphysics;
|
||
|
||
/**
|
||
*
|
||
*/
|
||
public class Body {
|
||
|
||
public static var linDamping:Number = 0.997;
|
||
public static var rotDamping:Number = 0.997;
|
||
|
||
// Идентификатор тела, уникальный в пределах мира
|
||
public var id:int;
|
||
// Имя тела
|
||
public var name:String;
|
||
// Мир, в котором находится тело
|
||
public var world:PhysicsScene;
|
||
// Флаг подвижности тела
|
||
public var movable:Boolean = true;
|
||
// Флаг указывает, может ли тело быть заморожено
|
||
public var canFreeze:Boolean = false;
|
||
|
||
public var freezeCounter:int;
|
||
public var frozen:Boolean = false;
|
||
// Ограничивающий бокс тела
|
||
public var aabb:BoundBox = new BoundBox();
|
||
// Предикат, через который тело получает сообщения о столкновениях
|
||
public var postCollisionPredicate:IBodyCollisionPredicate;
|
||
|
||
// Текущее состояние тела
|
||
public var state:BodyState = new BodyState();
|
||
// Предыдущее состояние тела
|
||
public var prevState:BodyState = new BodyState();
|
||
// Линейное ускорение тела на текущем шаге симуляции
|
||
public var accel:Vector3 = new Vector3();
|
||
// Угловое ускорение тела на текущем шаге симуляции
|
||
public var angleAccel:Vector3 = new Vector3();
|
||
// Физический материал тела
|
||
public var material:BodyMaterial = new BodyMaterial();
|
||
// Обратная масса тела
|
||
public var invMass:Number = 1;
|
||
// Обратная матрица тензора инерции в локальных координатах
|
||
public var invInertia:Matrix3 = new Matrix3();
|
||
// Обратная матрица тензора инерции в мировых координатах
|
||
public var invInertiaWorld:Matrix3 = new Matrix3();
|
||
// Базисная матрица тела в мировых координатах
|
||
public var baseMatrix:Matrix3 = new Matrix3();
|
||
|
||
public const MAX_CONTACTS:int = 20;
|
||
public var contacts:Vector.<Contact> = new Vector.<Contact>(MAX_CONTACTS);
|
||
public var contactsNum:int;
|
||
|
||
public var collisionPrimitives:CollisionPrimitiveList;
|
||
|
||
// Аккумулятор сил
|
||
public var forceAccum:Vector3 = new Vector3();
|
||
// Аккумулятор моментов
|
||
public 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 primitive
|
||
* @param localTransform
|
||
*/
|
||
public function addCollisionPrimitive(primitive:CollisionPrimitive, localTransform:Matrix4 = null):void {
|
||
if (primitive == null) {
|
||
throw new ArgumentError("Primitive cannot be null");
|
||
}
|
||
if (collisionPrimitives == null) {
|
||
collisionPrimitives = new CollisionPrimitiveList();
|
||
}
|
||
collisionPrimitives.append(primitive);
|
||
primitive.setBody(this, localTransform);
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param primitive
|
||
*/
|
||
public function removeCollisionPrimitive(primitive:CollisionPrimitive):void {
|
||
if (collisionPrimitives == null) return;
|
||
primitive.setBody(null);
|
||
collisionPrimitives.remove(primitive);
|
||
if (collisionPrimitives.size == 0) {
|
||
collisionPrimitives = null;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @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 x:Number = (r.y*dir.z - r.z*dir.y)*magnitude;
|
||
var y:Number = (r.z*dir.x - r.x*dir.z)*magnitude;
|
||
var z:Number = (r.x*dir.y - r.y*dir.x)*magnitude;
|
||
|
||
state.rotation.x += invInertiaWorld.a*x + invInertiaWorld.b*y + invInertiaWorld.c*z;
|
||
state.rotation.y += invInertiaWorld.e*x + invInertiaWorld.f*y + invInertiaWorld.g*z;
|
||
state.rotation.z += invInertiaWorld.i*x + invInertiaWorld.j*y + invInertiaWorld.k*z;
|
||
}
|
||
|
||
/**
|
||
* @param f
|
||
*/
|
||
public function addForce(f:Vector3):void {
|
||
forceAccum.vAdd(f);
|
||
}
|
||
|
||
/**
|
||
*
|
||
* @param fx
|
||
* @param fy
|
||
* @param fz
|
||
*/
|
||
public function addForceXYZ(fx:Number, fy:Number, fz:Number):void {
|
||
forceAccum.x += fx;
|
||
forceAccum.y += fy;
|
||
forceAccum.z += fz;
|
||
}
|
||
|
||
/**
|
||
* @param pos
|
||
* @param f
|
||
*/
|
||
public function addWorldForceXYZ(px:Number, py:Number, pz:Number, fx:Number, fy:Number, fz:Number):void {
|
||
forceAccum.x += fx;
|
||
forceAccum.y += fy;
|
||
forceAccum.z += fz;
|
||
|
||
var pos:Vector3 = state.pos;
|
||
var rx:Number = px - pos.x;
|
||
var ry:Number = py - pos.y;
|
||
var rz:Number = pz - pos.z;
|
||
|
||
// var x:Number = ry*fz - rz*fy;
|
||
// var y:Number = rz*fx - rx*fz;
|
||
// var z:Number = rx*fy - ry*fx;
|
||
|
||
torqueAccum.x += ry*fz - rz*fy;
|
||
torqueAccum.y += rz*fx - rx*fz;
|
||
torqueAccum.z += rx*fy - ry*fx;
|
||
}
|
||
|
||
/**
|
||
* @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 beforePhysicsStep(dt:Number):void {
|
||
}
|
||
|
||
/**
|
||
* @param t
|
||
*/
|
||
public function addTorque(t:Vector3):void {
|
||
torqueAccum.vAdd(t);
|
||
}
|
||
|
||
/**
|
||
*
|
||
*/
|
||
altphysics function clearAccumulators():void {
|
||
forceAccum.x = forceAccum.y = forceAccum.z = 0;
|
||
torqueAccum.x = torqueAccum.y = torqueAccum.z = 0;
|
||
}
|
||
|
||
/**
|
||
*
|
||
*/
|
||
altphysics function calcAccelerations():void {
|
||
accel.x = forceAccum.x*invMass;
|
||
accel.y = forceAccum.y*invMass;
|
||
accel.z = forceAccum.z*invMass;
|
||
angleAccel.x = invInertiaWorld.a*torqueAccum.x + invInertiaWorld.b*torqueAccum.y + invInertiaWorld.c*torqueAccum.z;
|
||
angleAccel.y = invInertiaWorld.e*torqueAccum.x + invInertiaWorld.f*torqueAccum.y + invInertiaWorld.g*torqueAccum.z;
|
||
angleAccel.z = invInertiaWorld.i*torqueAccum.x + invInertiaWorld.j*torqueAccum.y + invInertiaWorld.k*torqueAccum.z;
|
||
}
|
||
|
||
/**
|
||
* Вычисляет производные данные.
|
||
*/
|
||
public function calcDerivedData():void {
|
||
// Вычисление базисной матрицы и обратного тензора инерции в мировых координатах
|
||
state.orientation.toMatrix3(baseMatrix);
|
||
invInertiaWorld.copy(invInertia).append(baseMatrix).prependTransposed(baseMatrix);
|
||
if (collisionPrimitives != null) {
|
||
aabb.infinity();
|
||
var item:CollisionPrimitiveListItem = collisionPrimitives.head;
|
||
while (item != null) {
|
||
var primitive:CollisionPrimitive = item.primitive;
|
||
primitive.transform.setFromMatrix3(baseMatrix, state.pos);
|
||
if (primitive.localTransform != null) {
|
||
primitive.transform.prepend(primitive.localTransform);
|
||
}
|
||
primitive.calculateAABB();
|
||
aabb.addBoundBox(primitive.aabb);
|
||
item = item.next;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
*
|
||
*/
|
||
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);
|
||
}
|
||
|
||
}
|
||
} |