public btTransform(btQuaternion q) : this(BulletPINVOKE.new_btTransform__SWIG_2(btQuaternion.getCPtr(q)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public btMatrix3x3(btQuaternion q) : this(BulletPINVOKE.new_btMatrix3x3__SWIG_1(btQuaternion.getCPtr(q)), true)
{
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public void setRotation(btQuaternion q)
{
BulletPINVOKE.btMatrix3x3_setRotation(swigCPtr, btQuaternion.getCPtr(q));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public void setMotorTarget(btQuaternion qAinB, float dt)
{
BulletPINVOKE.btHingeConstraint_setMotorTarget__SWIG_0(swigCPtr, btQuaternion.getCPtr(qAinB), dt);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public static void calculateDiffAxisAngleQuaternion(btQuaternion orn0, btQuaternion orn1a, btVector3 axis, SWIGTYPE_p_float angle)
{
BulletPINVOKE.btTransformUtil_calculateDiffAxisAngleQuaternion(btQuaternion.getCPtr(orn0), btQuaternion.getCPtr(orn1a), btVector3.getCPtr(axis), SWIGTYPE_p_float.getCPtr(angle));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public void setMotorTargetInConstraintSpace(btQuaternion q)
{
BulletPINVOKE.btConeTwistConstraint_setMotorTargetInConstraintSpace(swigCPtr, btQuaternion.getCPtr(q));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
public BulletDotNETScene(string sceneIdentifier)
{
BulletLock = new object();
// m_sceneIdentifier = sceneIdentifier;
VectorZero = new btVector3(0, 0, 0);
QuatIdentity = new btQuaternion(0, 0, 0, 1);
TransZero = new btTransform(QuatIdentity, VectorZero);
m_gravity = new btVector3(0, 0, gravityz);
}
public BulletDotNETScene(string sceneIdentifier)
{
// m_sceneIdentifier = sceneIdentifier;
VectorZero = new btVector3(0, 0, 0);
QuatIdentity = new btQuaternion(0, 0, 0, 1);
TransZero = new btTransform(QuatIdentity, VectorZero);
m_gravity = new btVector3(0, 0, gravityz);
_origheightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize];
}
public btQuaternion nearest(btQuaternion qd)
{
btQuaternion ret = new btQuaternion(BulletPINVOKE.btQuaternion_nearest(swigCPtr, btQuaternion.getCPtr(qd)), true);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static btQuaternion shortestArcQuatNormalize2(btVector3 v0, btVector3 v1)
{
btQuaternion ret = new btQuaternion(BulletPINVOKE.shortestArcQuatNormalize2(btVector3.getCPtr(v0), btVector3.getCPtr(v1)), true);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static btVector3 quatRotate(btQuaternion rotation, btVector3 v)
{
btVector3 ret = new btVector3(BulletPINVOKE.quatRotate(btQuaternion.getCPtr(rotation), btVector3.getCPtr(v)), true);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static btQuaternion slerp(btQuaternion q1, btQuaternion q2, float t)
{
btQuaternion ret = new btQuaternion(BulletPINVOKE.slerp(btQuaternion.getCPtr(q1), btQuaternion.getCPtr(q2), t), true);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static btQuaternion inverse(btQuaternion q)
{
btQuaternion ret = new btQuaternion(BulletPINVOKE.inverse(btQuaternion.getCPtr(q)), true);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static float btAngle(btQuaternion q1, btQuaternion q2)
{
float ret = BulletPINVOKE.btAngle__SWIG_1(btQuaternion.getCPtr(q1), btQuaternion.getCPtr(q2));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public static float length(btQuaternion q)
{
float ret = BulletPINVOKE.length(btQuaternion.getCPtr(q));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public float dot(btQuaternion q)
{
float ret = BulletPINVOKE.btQuaternion_dot(swigCPtr, btQuaternion.getCPtr(q));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public btQuaternion slerp(btQuaternion q, float t)
{
btQuaternion ret = new btQuaternion(BulletPINVOKE.btQuaternion_slerp(swigCPtr, btQuaternion.getCPtr(q), t), true);
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public override void Dispose()
{
disposeAllBodies();
m_world.Dispose();
m_broadphase.Dispose();
((btDefaultCollisionConfiguration)m_collisionConfiguration).Dispose();
((btSequentialImpulseConstraintSolver)m_solver).Dispose();
worldAabbMax.Dispose();
worldAabbMin.Dispose();
VectorZero.Dispose();
QuatIdentity.Dispose();
m_gravity.Dispose();
VectorZero = null;
QuatIdentity = null;
}
// private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
public BulletDotNETCharacter(string avName, BulletDotNETScene parent_scene, PhysicsVector pos, PhysicsVector size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
{
m_taintedForce = new PhysicsVector();
m_velocity = new PhysicsVector();
m_target_velocity = new PhysicsVector();
m_position = pos;
m_zeroPosition = new PhysicsVector(pos.X, pos.Y, pos.Z); // this is a class, not a struct. Must make new, or m_zeroPosition will == position regardless
m_acceleration = new PhysicsVector();
m_parent_scene = parent_scene;
PID_D = pid_d;
PID_P = pid_p;
CAPSULE_RADIUS = capsule_radius;
m_density = density;
heightFudgeFactor = height_fudge_factor;
walkDivisor = walk_divisor;
runDivisor = rundivisor;
for (int i = 0; i < 11; i++)
{
m_colliderarr[i] = false;
}
for (int i = 0; i < 11; i++)
{
m_colliderGroundarr[i] = false;
}
CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
m_isPhysical = false; // current status: no ODE information exists
m_tainted_isPhysical = true; // new tainted status: need to create ODE information
m_parent_scene.AddPhysicsActorTaint(this);
// m_name = avName;
tempVector1 = new btVector3(0, 0, 0);
tempVector2 = new btVector3(0, 0, 0);
tempVector3 = new btVector3(0, 0, 0);
tempVector4 = new btVector3(0, 0, 0);
tempVector5RayCast = new btVector3(0, 0, 0);
tempVector6RayCast = new btVector3(0, 0, 0);
tempVector7RayCast = new btVector3(0, 0, 0);
tempQuat1 = new btQuaternion(0, 0, 0, 1);
tempTrans1 = new btTransform(tempQuat1, tempVector1);
// m_movementComparision = new PhysicsVector(0, 0, 0);
m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
}
public static void integrateTransform( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel
, double timeStep, out btTransform predictedTransform )
{
btVector3 tmp;
btVector3 tmp2;
linvel.Mult( timeStep, out tmp );
curTrans.getOrigin( out tmp2 );
tmp2.Add( ref tmp, out predictedTransform.m_origin );
#if QUATERNION_DERIVATIVE
btQuaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel predictedOrn) * (timeStep 0.5);
predictedOrn.normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
btVector3 axis;
double fAngle = angvel.length();
//limit the angular motion
if( fAngle * timeStep > ANGULAR_MOTION_THRESHOLD )
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if( fAngle < 0.001 )
{
// use Taylor's expansions of sync function
angvel.Mult( ( btScalar.BT_HALF * timeStep - ( timeStep * timeStep * timeStep ) * ( 0.020833333333 ) * fAngle * fAngle ), out axis );
}
else
{
// sync(fAngle) = sin(cfAngle)/t
angvel.Mult( ( btScalar.btSin( btScalar.BT_HALF * fAngle * timeStep ) / fAngle ), out axis );
}
btQuaternion dorn = new btQuaternion( axis.x, axis.y, axis.z, btScalar.btCos( fAngle * timeStep * 0.5 ) );
btQuaternion orn0;
curTrans.getRotation( out orn0 );
btQuaternion predictedOrn;
dorn.Mult( ref orn0, out predictedOrn );
predictedOrn.normalize();
#endif
btMatrix3x3.setRotation( out predictedTransform.m_basis, ref predictedOrn);
//predictedTransform.setRotation( ref predictedOrn );
}
internal static void calculateVelocityQuaternion( ref btVector3 pos0, ref btVector3 pos1, ref btQuaternion orn0, ref btQuaternion orn1, double timeStep, out btVector3 linVel, out btVector3 angVel )
{
btVector3 tmp;
pos1.Sub( ref pos0, out tmp );
tmp.Div( timeStep, out linVel );
btVector3 axis;
double angle;
if( !orn0.Equals(ref orn1 ) )
{
calculateDiffAxisAngleQuaternion( ref orn0, ref orn1, out axis, out angle );
axis.Mult( angle, out tmp );
tmp.Div( timeStep, out angVel );
}
else
{
angVel = btVector3.Zero;
}
}
private void LimitRotation(float timestep)
{
btQuaternion rot = m_body.getWorldTransform().getRotation();
bool changed = false;
if (m_RollreferenceFrame != Quaternion.Identity)
{
if (rot.getX() >= m_RollreferenceFrame.X)
{
rot = new btQuaternion(rot.getX() - (m_RollreferenceFrame.X / 2), rot.getY(), rot.getZ(), rot.getW());
}
if (rot.getX() <= -m_RollreferenceFrame.X)
{
rot = new btQuaternion(rot.getX() + (m_RollreferenceFrame.X / 2), rot.getY(), rot.getZ(), rot.getW());
}
if (rot.getY() >= m_RollreferenceFrame.Y)
{
rot = new btQuaternion(rot.getX(), rot.getY() - (m_RollreferenceFrame.Y / 2), rot.getZ(), rot.getW());
}
if (rot.getY() <= -m_RollreferenceFrame.Y)
{
rot = new btQuaternion(rot.getX(), rot.getY() + (m_RollreferenceFrame.Y / 2), rot.getZ(), rot.getW());
}
changed = true;
}
if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
{
rot = new btQuaternion(0, 0, rot.getZ(), rot.getW());
changed = true;
}
if (changed)
{
btTransform trans = m_body.getWorldTransform();
trans.setRotation(rot);
m_body.setWorldTransform(trans);
}
}
public static void quatRotate( ref btQuaternion rotation, ref btVector3 v, out btVector3 result )
{
btQuaternion q;
btQuaternion tmp;
rotation.Mult( ref v, out q );
rotation.inverse( out tmp );
q.Mult( ref tmp, out result );
}
/*@brief Return the inverse of a quaternion*/
public static void inverse( ref btQuaternion q, out btQuaternion result )
{
q.inverse( out result );
}
/*@brief Return the length of a quaternion */
public double length( ref btQuaternion q )
{
return q.length();
}
public static void Apply( ref btVector3 w, ref btQuaternion q, out btQuaternion result )
{
result.x = +w.x * q.w + w.y * q.z - w.z * q.y;
result.y = +w.y * q.w + w.z * q.x - w.x * q.z;
result.z = +w.z * q.w + w.x * q.y - w.y * q.x;
result.w = -w.x * q.x - w.y * q.y - w.z * q.z;
}
/*@brief Return the angle between this quaternion and the other along the shortest path
@param q The other quaternion */
public double angleShortestPath( ref btQuaternion q )
{
double s = btScalar.btSqrt( length2() * q.length2() );
//Debug.Assert( s != 0.0 );
if( dot( ref q ) < 0 ) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
{
btQuaternion b;
q.inverse( out b );
return btScalar.btAcos( dot( ref b ) / s ) * 2.0;
}
else
return btScalar.btAcos( dot( ref q ) / s ) * 2.0;
}
/*@brief Return a normalized version of this quaternion */
void normalized( out btQuaternion result )
{
Div( ref this, length(), out result );
}
/*@brief Add two quaternions
@param q The quaternion to add to this one */
public void Add( ref btQuaternion q2, out btQuaternion result )
{
result.x = x + q2.x;
result.y = y + q2.y;
result.z = z + q2.z;
result.w = w + q2.w;
}
public void ProcessTaints(float timestep)
{
if (m_tainted_isPhysical != m_isPhysical)
{
if (m_tainted_isPhysical)
{
// Create avatar capsule and related ODE data
if (!(Shell == null && Body == null))
{
m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
+ (Shell != null ? "Shell " : "")
+ (Body != null ? "Body " : ""));
}
AvatarGeomAndBodyCreation(m_position.X, m_position.Y, m_position.Z);
}
else
{
// destroy avatar capsule and related ODE data
Dispose();
tempVector1 = new btVector3(0, 0, 0);
tempVector2 = new btVector3(0, 0, 0);
tempVector3 = new btVector3(0, 0, 0);
tempVector4 = new btVector3(0, 0, 0);
tempVector5RayCast = new btVector3(0, 0, 0);
tempVector6RayCast = new btVector3(0, 0, 0);
tempVector7RayCast = new btVector3(0, 0, 0);
tempQuat1 = new btQuaternion(0, 0, 0, 1);
tempTrans1 = new btTransform(tempQuat1, tempVector1);
// m_movementComparision = new PhysicsVector(0, 0, 0);
m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
}
m_isPhysical = m_tainted_isPhysical;
}
if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
{
if (Body != null)
{
m_pidControllerActive = true;
// no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
//d.JointDestroy(Amotor);
float prevCapsule = CAPSULE_LENGTH;
CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
//m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
Dispose();
tempVector1 = new btVector3(0, 0, 0);
tempVector2 = new btVector3(0, 0, 0);
tempVector3 = new btVector3(0, 0, 0);
tempVector4 = new btVector3(0, 0, 0);
tempVector5RayCast = new btVector3(0, 0, 0);
tempVector6RayCast = new btVector3(0, 0, 0);
tempVector7RayCast = new btVector3(0, 0, 0);
tempQuat1 = new btQuaternion(0, 0, 0, 1);
tempTrans1 = new btTransform(tempQuat1, tempVector1);
// m_movementComparision = new PhysicsVector(0, 0, 0);
m_CapsuleOrientationAxis = new btVector3(1, 0, 1);
AvatarGeomAndBodyCreation(m_position.X, m_position.Y,
m_position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
Velocity = Vector3.Zero;
}
else
{
m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
+ (Shell == null ? "Shell " : "")
+ (Body == null ? "Body " : ""));
}
}
if (m_taintRemove)
{
Dispose();
}
}
/// <summary>
/// This creates the Avatar's physical Surrogate at the position supplied
/// </summary>
/// <param name="npositionX"></param>
/// <param name="npositionY"></param>
/// <param name="npositionZ"></param>
// WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
// to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
// place that is safe to call this routine AvatarGeomAndBodyCreation.
private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
{
if (CAPSULE_LENGTH <= 0)
{
m_log.Warn("[PHYSICS]: The capsule size you specified in aurora.ini is invalid! Setting it to the smallest possible size!");
CAPSULE_LENGTH = 0.01f;
}
if (CAPSULE_RADIUS <= 0)
{
m_log.Warn("[PHYSICS]: The capsule size you specified in aurora.ini is invalid! Setting it to the smallest possible size!");
CAPSULE_RADIUS = 0.01f;
}
Shell = new btCapsuleShape(CAPSULE_RADIUS, CAPSULE_LENGTH);
if (m_bodyPosition == null)
{
m_bodyPosition = new btVector3(npositionX, npositionY, npositionZ);
}
m_bodyPosition.setValue(npositionX, npositionY, npositionZ);
if (m_bodyOrientation == null)
{
m_bodyOrientation = new btQuaternion(m_CapsuleOrientationAxis, (Utils.DEG_TO_RAD * 90));
}
if (m_bodyTransform == null)
{
m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition);
}
else
{
m_bodyTransform.Dispose();
m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition);
}
if (m_bodyMotionState == null)
{
m_bodyMotionState = new btDefaultMotionState(m_bodyTransform);
}
else
{
m_bodyMotionState.setWorldTransform(m_bodyTransform);
}
m_mass = Mass;
Body = new btRigidBody(m_mass, m_bodyMotionState, Shell);
// this is used for self identification. User localID instead of body handle
Body.setUserPointer(new IntPtr((int)m_localID));
if (ClosestCastResult != null)
{
ClosestCastResult.Dispose();
}
ClosestCastResult = new ClosestNotMeRayResultCallback(Body);
m_parent_scene.AddRigidBody(Body);
Body.setActivationState(4);
if (m_aMotor != null)
{
if (m_aMotor.Handle != IntPtr.Zero)
{
m_parent_scene.getBulletWorld().removeConstraint(m_aMotor);
m_aMotor.Dispose();
}
m_aMotor = null;
}
m_aMotor = new btGeneric6DofConstraint(Body, m_parent_scene.TerrainBody,
m_parent_scene.TransZero,
m_parent_scene.TransZero, false);
m_aMotor.setAngularLowerLimit(m_parent_scene.VectorZero);
m_aMotor.setAngularUpperLimit(m_parent_scene.VectorZero);
}
static void Main(string[] args)
{
btVector3 testvec = new btVector3(-2, 1, 0);
Console.WriteLine(String.Format("Original: {0}", testvec.testStr()));
btVector3 testvec2 = testvec.absolute();
Console.WriteLine(String.Format("absolute: {0}", testvec2.testStr()));
Console.WriteLine(String.Format("angle:{0}", testvec.angle(testvec2)));
Console.WriteLine(String.Format("closestAxis(orig):{0}", testvec.closestAxis()));
btVector3 testvec3 = testvec.cross(testvec2);
Console.WriteLine(String.Format("cross: {0}", testvec3.testStr()));
Console.WriteLine(String.Format("distance: {0}", testvec.distance(testvec2)));
Console.WriteLine(String.Format("distance2: {0}", testvec.distance2(testvec2)));
Console.WriteLine(String.Format("dot: {0}", testvec.dot(testvec2)));
Console.WriteLine(String.Format("furthestAxis(orig): {0}", testvec.furthestAxis()));
btVector3 testvec4 = testvec.normalized();
Console.WriteLine(String.Format("normalized: {0}", testvec4.testStr()));
testvec4.setInterpolate3(testvec, testvec2, 0.5f);
Console.WriteLine(String.Format("interpolate3: {0}", testvec4.testStr()));
testvec4.setValue(7f, -0.09f, 2.5f);
Console.WriteLine(String.Format("setvec: {0}", testvec4.testStr()));
testvec4.setX(5.0f);
testvec4.setY(-0.25f);
testvec4.setZ(90f);
testvec.setValue(0, 0, -1024);
testvec2.setValue(256, 256, 1024);
Console.WriteLine(String.Format("setvecIndividual: {0}", testvec4.testStr()));
btAxisSweep3 testbtAxisSweep3 = new btAxisSweep3(testvec, testvec2, 50);
btDefaultCollisionConfiguration colconfig = new btDefaultCollisionConfiguration();
btCollisionDispatcher coldisp = new btCollisionDispatcher(colconfig);
btSequentialImpulseConstraintSolver seqimpconssol = new btSequentialImpulseConstraintSolver();
btDiscreteDynamicsWorld dynamicsWorld = new btDiscreteDynamicsWorld(coldisp, testbtAxisSweep3, seqimpconssol,
colconfig);
dynamicsWorld.setGravity(new btVector3(0, 0, -9.87f));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
Console.WriteLine(String.Format("stepWorld: {0}", dynamicsWorld.stepSimulation((6f / 60), 5, (1f / 60))));
btQuaternion testquat = new btQuaternion(50, 0, 0, 1);
btQuaternion testquatnorm = testquat.normalized();
Console.WriteLine(String.Format("testquat: {0}", testquat.testStr()));
Console.WriteLine(String.Format("testquatnormalize: {0}", testquatnorm.testStr()));
Console.WriteLine(String.Format("testquatLength: {0}", testquat.length()));
Console.WriteLine(String.Format("testquatnormalizeLength: {0}", testquatnorm.length()));
float[] heightdata = new float[256 * 256];
for (int j = 0; j < 256 * 256; j++)
{
if (j % 2 == 0)
{
heightdata[j] = 21f;
}
else
{
heightdata[j] = 28f;
}
}
btHeightfieldTerrainShape obj = new btHeightfieldTerrainShape(256, 256, heightdata, 1.0f, 0, 256,
(int)btHeightfieldTerrainShape.UPAxis.Z,
(int)btHeightfieldTerrainShape.PHY_ScalarType.
PHY_FLOAT, false);
btCapsuleShape cap = new btCapsuleShape(0.23f, 3);
btTriangleMesh testMesh = new btTriangleMesh(true, false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, 0, 1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(-1, 0, -1), new btVector3(-1, -1, -1), false);
testMesh.addTriangle(new btVector3(1, -1, -1), new btVector3(1, 0, -1), new btVector3(-1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, 0, 1), new btVector3(1, -1, -1), new btVector3(1, 0, -1), false);
testMesh.addTriangle(new btVector3(1, -1, 1), new btVector3(1, -1, -1), new btVector3(1, 0, 1), false);
btGImpactMeshShape meshtest = new btGImpactMeshShape(testMesh);
meshtest.updateBound();
btRigidBody groundbody = new btRigidBody(0,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 256f / 2f))), obj,
new btVector3(0, 0, 0));
btRigidBody capbody = new btRigidBody(200,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 25))), cap,
new btVector3(0, 0, 0));
btRigidBody meshbody = new btRigidBody(200,
new btDefaultMotionState(
new btTransform(new btQuaternion(0, 0, 0, 1),
new btVector3(128, 128, 29))), meshtest,
new btVector3(0, 0, 0));
btRigidBodyConstructionInfo constructioninfotest = new btRigidBodyConstructionInfo();
constructioninfotest.m_collisionShape = new btBoxShape(new btVector3(0.5f, 0.5f, 0.5f));
constructioninfotest.m_localInertia = new btVector3(0, 0, 0);
constructioninfotest.m_motionState = new btDefaultMotionState(new btTransform(new btQuaternion(0.3f, -0.4f, 0.8f, 0.1f), new btVector3(128.5f, 128, 25)),
new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0.25f, 0)));
constructioninfotest.m_startWorldTransform = new btTransform(new btQuaternion(0, 0, 0, 1), new btVector3(0, 0, 0));
constructioninfotest.m_mass = 2000000;
constructioninfotest.m_linearDamping = 0;
constructioninfotest.m_angularDamping = 0;
constructioninfotest.m_friction = 0.1f;
constructioninfotest.m_restitution = 0;
constructioninfotest.m_linearSleepingThreshold = 0.8f;
constructioninfotest.m_angularSleepingThreshold = 1;
constructioninfotest.m_additionalDamping = false;
constructioninfotest.m_additionalDampingFactor = 0.005f;
constructioninfotest.m_additionalLinearDampingThresholdSqr = 0.01f;
constructioninfotest.m_additionalAngularDampingThresholdSqr = 0.01f;
constructioninfotest.m_additionalAngularDampingFactor = 0.01f;
constructioninfotest.commit();
btGImpactCollisionAlgorithm.registerAlgorithm(coldisp);
btRigidBody cubetest = new btRigidBody(constructioninfotest);
dynamicsWorld.addRigidBody(groundbody);
dynamicsWorld.addRigidBody(cubetest);
dynamicsWorld.addRigidBody(capbody);
dynamicsWorld.addRigidBody(meshbody);
int frame = 0;
for (int i = 0; i < 26; i++)
{
int frames = dynamicsWorld.stepSimulation(((i % 60) / 60f), 10, (1f / 60));
frame += frames;
Console.WriteLine(String.Format("Cube: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, cubetest.getInterpolationWorldTransform().getOrigin().testStr(), cubetest.getWorldTransform().getRotation().testStr()));
Console.WriteLine(String.Format("Cap: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, capbody.getInterpolationWorldTransform().getOrigin().testStr(), capbody.getWorldTransform().getRotation().testStr()));
Console.WriteLine(String.Format("Mesh: frame {0} frames: {1} POS:{2}, quat:{3}", frame, frames, meshbody.getInterpolationWorldTransform().getOrigin().testStr(), meshbody.getWorldTransform().getRotation().testStr()));
}
dynamicsWorld.removeRigidBody(meshbody);
dynamicsWorld.removeRigidBody(capbody);
dynamicsWorld.removeRigidBody(cubetest);
dynamicsWorld.removeRigidBody(groundbody);
cubetest.Dispose();
groundbody.Dispose();
capbody.Dispose();
cap.Dispose();
obj.Dispose();
testbtAxisSweep3.Dispose();
dynamicsWorld.Dispose();
coldisp.Dispose();
colconfig.Dispose();
seqimpconssol.Dispose();
testvec.Dispose();
testvec2.Dispose();
testvec3.Dispose();
testvec4.Dispose();
}
private void MoveLinear(float timestep)
{
if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
{
// add drive to body
Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / timestep);
m_lastLinearVelocityVector += (addAmount * 10); // lastLinearVelocityVector is the current body velocity vector?
// This will work temporarily, but we really need to compare speed on an axis
// KF: Limit body velocity to applied velocity?
if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
{
m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
}
if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
{
m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
}
if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
{
m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
}
// decay applied velocity
Vector3 decayfraction = ((Vector3.One / (m_linearMotorDecayTimescale / timestep)));
//Console.WriteLine("decay: " + decayfraction);
m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
//Console.WriteLine("actual: " + m_linearMotorDirection);
}
else
{ // requested is not significant
// if what remains of applied is small, zero it.
if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
{
m_lastLinearVelocityVector = Vector3.Zero;
}
}
// convert requested object velocity to world-referenced vector
m_dir = m_lastLinearVelocityVector;
btQuaternion rot = m_body.getWorldTransform().getRotation();
Quaternion rotq = new Quaternion(rot.getX(), rot.getY(), rot.getZ(), rot.getW()); // rotq = rotation of object
m_dir *= rotq; // apply obj rotation to velocity vector
// add Gravity andBuoyancy
// KF: So far I have found no good method to combine a script-requested
// .Z velocity and gravity. Therefore only 0g will used script-requested
// .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
Vector3 grav = Vector3.Zero;
// There is some gravity, make a gravity force vector
// that is applied after object velocity.
float objMass = m_prim.Mass;
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
//Rev: bullet does gravity internally
grav.Z = -parent_scene.gravityz * objMass * m_VehicleBuoyancy; //parent_scene.gravityz/* * objMass*/ * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
btVector3 pos = m_body.getWorldTransform().getOrigin();
btVector3 newpos = pos;
m_dir.Z = m_prim.Velocity.Z; // Preserve the accumulated falling velocity
Vector3 posChange = new Vector3();
posChange.X = newpos.getX() - m_lastPositionVector.getX();
posChange.Y = newpos.getY() - m_lastPositionVector.getY();
posChange.Z = newpos.getZ() - m_lastPositionVector.getZ();
btQuaternion Orientation2 = m_body.getWorldTransform().getRotation();
/*if (m_BlockingEndPoint != Vector3.Zero)
* {
* if (newpos.getX() >= (m_BlockingEndPoint.X - (float)1))
* newpos.setX(newpos.getX() - (posChange.X + 1));
* if (newpos.getY() >= (m_BlockingEndPoint.Y - (float)1))
* newpos.setY(newpos.getY() - (posChange.Y + 1));
* if (newpos.getZ() >= (m_BlockingEndPoint.Z - (float)1))
* newpos.setZ(newpos.getZ() - (posChange.Z + 1));
* if (newpos.getX() <= 0)
* newpos.setX(newpos.getX() + (posChange.X + 1));
* if (newpos.getY() <= 0)
* newpos.setY(newpos.getY() + (posChange.Y + 1));
* }
*/
if (newpos.getZ() < parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY()))
{
newpos.setZ(parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY()) + 2);
}
// Check if hovering
if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
{
float diff = (newpos.getZ() - m_VhoverTargetHeight);
// We should hover, get the target height
if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
{
m_VhoverTargetHeight = parent_scene.GetWaterLevel() + m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
{
m_VhoverTargetHeight = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY()) + m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
{
m_VhoverTargetHeight = m_VhoverHeight;
}
if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
{
// If body is aready heigher, use its height as target height
if (newpos.getZ() > m_VhoverTargetHeight)
{
m_VhoverTargetHeight = newpos.getZ();
}
}
if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if (diff > .2 || diff < -.2)
{
newpos.setValue(newpos.getX(), newpos.getY(), m_VhoverTargetHeight);
btTransform trans = new btTransform(Orientation2, newpos);
m_body.setWorldTransform(trans);
}
}
else
{
// Replace Vertical speed with correction figure if significant
if (Math.Abs(diff) > 0.01f)
{
m_dir.Z = -((diff * timestep * 50.0f) / m_VhoverTimescale);
}
else
{
m_dir.Z = 0f;
}
}
}
/*if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
* {
* //Start Experimental Values
* if (Zchange > .3)
* grav.Z = (float)(grav.Z * 3);
* if (Zchange > .15)
* grav.Z = (float)(grav.Z * 2);
* if (Zchange > .75)
* grav.Z = (float)(grav.Z * 1.5);
* if (Zchange > .05)
* grav.Z = (float)(grav.Z * 1.25);
* if (Zchange > .025)
* grav.Z = (float)(grav.Z * 1.125);
*
* float terraintemp = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY());
* float postemp = (pos.getZ() - terraintemp);
*
* if (postemp > 2.5f)
* grav.Z = (float)(grav.Z * 1.037125);
* //End Experimental Values
* }*/
if ((m_flags & (VehicleFlag.NO_X)) != 0)
{
m_dir.X = 0;
}
if ((m_flags & (VehicleFlag.NO_Y)) != 0)
{
m_dir.Y = 0;
}
if ((m_flags & (VehicleFlag.NO_Z)) != 0)
{
m_dir.Z = 0;
}
m_lastPositionVector = new btVector3(m_prim.Position.X, m_prim.Position.Y, m_prim.Position.Z);
// Apply velocity
//if(m_dir != Vector3.Zero)
// m_body.setLinearVelocity(new btVector3(m_dir.X, m_dir.Y, m_dir.Z));
m_body.applyCentralImpulse(new btVector3(m_dir.X, m_dir.Y, m_dir.Z));
// apply gravity force
//m_body.applyCentralImpulse(new btVector3(0, 0, 9.8f));
/*ector3 newpos2 = new Vector3(newpos.getX(), newpos.getY(), newpos.getZ());
* if (newpos2.X != m_prim.Position.X || newpos2.Y != m_prim.Position.Y || newpos2.Z != m_prim.Position.Z)
* {
* btTransform trans = new btTransform(Orientation2, newpos);
* m_body.setWorldTransform(trans);
* }*/
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / timestep);
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
}
private void MoveAngular(float timestep)
{
/*
* private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
* private int m_angularMotorApply = 0; // application frame counter
* private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
* private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
* private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
* private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
* private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
*/
// Get what the body is doing, this includes 'external' influences
btVector3 angularVelocity = m_body.getInterpolationAngularVelocity();
// Vector3 angularVelocity = Vector3.Zero;
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / timestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / timestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / timestep);
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
}
else
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
* m_angularMotorVelocity.Y = angularVelocity.Y;
* m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / timestep);
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
if (m_verticalAttractionTimescale < 300)
{
float VAservo = 0.2f / (m_verticalAttractionTimescale * timestep);
// get present body rotation
btQuaternion rot = m_body.getWorldTransform().getRotation();
Quaternion rotq = new Quaternion(rot.getX(), rot.getY(), rot.getZ(), rot.getW());
// make a vector pointing up
Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f;
// rotate it to Body Angle
verterr = verterr * rotq;
// verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
// As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if (verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo
verterr = verterr * VAservo;
//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = -verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.getX();
vertattr.Y += bounce * angularVelocity.getY();
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// Bank section tba
// Deflection section tba
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
}
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / timestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
m_body.setAngularVelocity(new btVector3(m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z));
}
public void shortestArcQuatNormalize2( ref btVector3 v0, ref btVector3 v1, out btQuaternion result )
{
btVector3 _v0;
btVector3 _v1;
v0.normalized( out _v0 );
v1.normalized( out _v1 );
shortestArcQuat( ref _v0, ref _v1, out result );
}
/*@brief Subtract out a quaternion
@param q The quaternion to subtract from this one */
public static void Sub( ref btQuaternion q1, ref btQuaternion q2, out btQuaternion result )
{
result.x = q1.x - q2.x;
result.y = q1.y - q2.y;
result.z = q1.z - q2.z;
result.w = q1.w - q2.w;
}
public static void calculateVelocityQuaternion(btVector3 pos0, btVector3 pos1, btQuaternion orn0, btQuaternion orn1, float timeStep, btVector3 linVel, btVector3 angVel)
{
BulletPINVOKE.btTransformUtil_calculateVelocityQuaternion(btVector3.getCPtr(pos0), btVector3.getCPtr(pos1), btQuaternion.getCPtr(orn0), btQuaternion.getCPtr(orn1), timeStep, btVector3.getCPtr(linVel), btVector3.getCPtr(angVel));
if (BulletPINVOKE.SWIGPendingException.Pending)
{
throw BulletPINVOKE.SWIGPendingException.Retrieve();
}
}
/*@brief Return the negative of this quaternion
* This simply negates each element */
public void Sub( btQuaternion q, out btQuaternion result )
{
result.x = x - q.x;
result.y = y - q.y;
result.z = z - q.z;
result.w = w - q.w;
}
/*@brief Return the []half[] angle between this quaternion and the other
@param q The other quaternion */
double angle( ref btQuaternion q )
{
double s = btScalar.btSqrt( length2() * q.length2() );
//Debug.Assert( s != 0.0 );
return btScalar.btAcos( dot( ref q ) / s );
}
/*@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
* @param q The other quaternion to interpolate with
* @param t The ratio between this and q to interpolate. * If t = 0 the result is this, if t=1 the result is q.
* Slerp interpolates assuming constant velocity. */
void slerp( ref btQuaternion q, double t, out btQuaternion result )
{
double magnitude = btScalar.btSqrt( length2() * q.length2() );
//Debug.Assert( magnitude > (double)( 0 ) );
double product = dot( ref q ) / magnitude;
if( btScalar.btFabs( product ) < 1 )
{
// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
double sign = ( product < 0 ) ? -1 : 1;
double theta = btScalar.btAcos( sign * product );
double s1 = btScalar.btSin( sign * t * theta );
double d = 1.0 / btScalar.btSin( theta );
double s0 = btScalar.btSin( ( 1.0 - t ) * theta );
result.x = ( x * s0 + q.x * s1 ) * d;
result.y = ( y * s0 + q.y * s1 ) * d;
result.z = ( z * s0 + q.z * s1 ) * d;
result.w = ( w * s0 + q.w * s1 ) * d;
}
else
{
result = this;
}
}
/*@brief Return the inverse of this quaternion */
public void inverse( out btQuaternion result )
{
result.x = -x;
result.y = -y;
result.z = -z;
result.w = w;
}
public btQuaternion getOrientation()
{
btQuaternion ret = new btQuaternion(BulletPINVOKE.btRigidBody_getOrientation(swigCPtr), true);
return(ret);
}
/*@todo document this and it's use */
public void nearest( ref btQuaternion qd, out btQuaternion result )
{
btQuaternion diff, sum;
this.Sub( ref qd, out diff );
this.Add( ref qd, out sum );
if( diff.dot( ref diff ) < sum.dot( ref sum ) )
result = qd;
qd.inverse( out result );
}
public static btQuaternion getIdentity()
{
btQuaternion ret = new btQuaternion(BulletPINVOKE.btQuaternion_getIdentity(), false);
return(ret);
}
//public double getw { return w; }
/*
public void serialize(struct btQuaternionData& dataOut);
public void deSerialize(stringstruct btQuaternionData& dataIn);
public void serializeFloat(struct btQuaternionFloatData& dataOut);
public void deSerializeFloat(stringstruct btQuaternionFloatData& dataIn);
public void serializeDouble(struct btQuaternionDoubleData& dataOut);
public void deSerializeDouble(stringstruct btQuaternionDoubleData& dataIn);
*/
public static void Apply( ref btQuaternion q, ref btVector3 w, out btQuaternion result )
{
result.x = q.w * w.x + q.y * w.z - q.z * w.y;
result.y = q.w * w.y + q.z * w.x - q.x * w.z;
result.z = q.w * w.z + q.x * w.y - q.y * w.x;
result.w = -q.x * w.x - q.y * w.y - q.z * w.z;
}
public void Mult( ref btQuaternion q2, out btQuaternion result )
{
result.x = w * q2.x + x * q2.w + y * q2.z - z * q2.y;
result.y = w * q2.y + y * q2.w + z * q2.x - x * q2.z;
result.z = w * q2.z + z * q2.w + x * q2.y - y * q2.x;
result.w = w * q2.w - x * q2.x - y * q2.y - z * q2.z;
}
/*@brief Calculate the dot product between two quaternions */
public static double dot( ref btQuaternion q1, ref btQuaternion q2 )
{
return q1.dot( ref q2 );
}
public void Mult( ref btVector3 w, out btQuaternion result )
{
result.x = this.w * w.x + y * w.z - z * w.y;
result.y = this.w * w.y + z * w.x - x * w.z;
result.z = this.w * w.z + x * w.y - y * w.x;
result.w = -x * w.x - y * w.y - z * w.z;
}
/*@brief Return the angle between two quaternions*/
public static double btAngle( ref btQuaternion q1, ref btQuaternion q2 )
{
return q1.angle( ref q2 );
}
/*@brief Return the dot product between this quaternion and another
@param q The other quaternion */
public double dot( ref btQuaternion q )
{
return x * q.x +
y * q.y +
z * q.z +
w * q.w;
}
/*@brief Return the result of spherical linear interpolation betwen two quaternions
@param q1 The first quaternion
@param q2 The second quaternion
@param t The ration between q1 and q2. t = 0 return q1, t=1 returns q2
Slerp assumes constant velocity between positions. */
public void slerp( ref btQuaternion q1, ref btQuaternion q2, double t, out btQuaternion result )
{
q1.slerp( ref q2, t, out result );
}
/*@brief Return a scaled version of this quaternion
@param s The scale factor */
public void Mult( double s, out btQuaternion result )
{
result.x = x * s;
result.y = y * s;
result.z = z * s;
result.w = w * s;
}
public static void shortestArcQuat( ref btVector3 v0, ref btVector3 v1, out btQuaternion result ) // Game Programming Gems 2.10 make sure v0,v1 are normalized
{
btVector3 c;
v0.cross( ref v1, out c );
double d = v0.dot( ref v1 );
if( d < -1.0 + btScalar.SIMD_EPSILON )
{
btVector3 n, unused;
btVector3.btPlaneSpace1( ref v0, out n, out unused );
result.x = n.x; result.y = n.y; result.z = n.z; result.w = 0.0f; // just pick any vector that is orthogonal to v0
return;
}
double s = btScalar.btSqrt( ( 1.0f + d ) * 2.0f );
double rs = 1.0f / s;
result.x = c.x * rs;
result.y = c.y * rs;
result.z = c.z * rs;
result.w = s * 0.5f;
}
/*@brief Return an inversely scaled versionof this quaternion
@param s The inverse scale factor */
public static void Div( ref btQuaternion q, double s, out btQuaternion result )
{
//Debug.Assert( s != 0.0 );
q.Mult( ( 1.0 / s ), out result );
}
public bool Equals( ref btQuaternion q )
{
return ( q.x == x ) && ( q.y == y ) && ( q.z == z ) && ( q.w == w );
}
public btQuaternion getRotation()
{
btQuaternion ret = new btQuaternion(BulletPINVOKE.btTransform_getRotation(swigCPtr), true);
return(ret);
}
public static void Mult( ref btQuaternion q, double s, out btVector3 result )
{
result.x = q.x * s;
result.y = q.y * s;
result.z = q.z * s;
result.w = q.w * s;
}
/*@brief Multiply this quaternion by q on the right
@param q The other quaternion
Equivilant to this = this * q */
public static void Mult( ref btQuaternion q1, ref btQuaternion q2, out btQuaternion result )
{
result.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
result.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
result.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
result.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
}
/*@brief Subtract out a quaternion
@param q The quaternion to subtract from this one */
public void Sub( ref btQuaternion q2, out btQuaternion result )
{
result.x = x - q2.x;
result.y = y - q2.y;
result.z = z - q2.z;
result.w = w - q2.w;
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(btQuaternion obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
