public Complex NextStepComponent (int j, Ode ode, double t, Complex[] y)
{
Complex[] ya = new Complex[y.Length];
Complex[] yb = new Complex[y.Length];
Complex[] yc = new Complex[y.Length];
Complex[] dya, dyb, dyc;
Complex[] dy = ode.Calc (t, y);
for (int i = 0; i < y.Length; i++) {
ya [i] = y [i] + .5 * _h * dy [i];
}
dya = ode.Calc (t + .5 * _h.Real, ya);
for (int i = 0; i < y.Length; i++) {
yb [i] = y [i] + .5 * _h * dya [i];
}
dyb = ode.Calc (t + .5 * _h.Real, yb);
for (int i = 0; i < y.Length; i++) {
yc [i] = y [i] + _h * dyb [i];
}
dyc = ode.Calc (t + _h.Real, yc);
return y [j] + _h * (dy [j] + 2 * (dya[j] + dyb[j]) + dyc[j]) / 6;
}
public void dBodyGetRotationTest()
{
IntPtr newWorldId = Ode.dWorldCreate();
IntPtr newBodyId = Ode.dBodyCreate(newWorldId);
Ode.dMatrix3 r = new Ode.dMatrix3(
new float[] {
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f
}
);
Ode.dMatrix3 r2
= new Ode.dMatrix3(
new float[] {
0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f
}
);
Ode.dBodySetRotation(newBodyId, r);
r2 = Ode.dBodyGetRotation(newBodyId);
Assert.AreEqual(r, r2, "Assigned and returned rotation matrix values are not equal");
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateSlider(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetSliderParam(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
axis.UpdateToLibrary(true);
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
public IReadOnlyList <WorkingProbability> GetProbability(double from, double to, double step)
{
var coefficients = BuildWeightMatrix();
// The initial vector describes the situation when the first state
// holds true - the state where all modules are working
var initial = new Vector(Enumerable.Repeat(0.0, AllPossibleStates.Count).ToArray())
{
[0] = 1.0
};
var solution = Ode.RK547M(
0,
initial,
(_, vector) => GetNextStateProbabilityDistribution(vector, coefficients));
var workingIndices = AllPossibleStates
.Select((state, index) => new { index, state.IsWorking })
.Where(state => state.IsWorking)
.Select(state => state.index)
.ToArray();
var points = solution.SolveFromToStep(from, to, step).ToArray();
return(points.Select(point => new WorkingProbability
{
Time = point.T,
AggregatedProbability = point.X.ToArray().Where((_, index) => workingIndices.Contains(index)).Sum()
}).ToList());
}
public void solve(ref List <double> tsim, ref List <double>[] xsim)
{
var plotLocs = new List <List <int> >();
plotLocs.Add(new List <int> {
76, 128, 143
});
plotLocs.Add(new List <int> {
44, 82, 97
});
plotLocs.Add(new List <int> {
40, 50, 56
});
double tfinal = 500000.0;
// Ode options and initial conditions, then solve
var opts = new Options {
RelativeTolerance = 1e-4
};
var x0 = oscillating_ics();
IEnumerable <SolPoint> sol;
sol = Ode.GearBDF(0.0, tfinal, x0, (t, x) => derivs(t, x), opts);
double dt;
// Write to variables
foreach (var sp in sol)
{
if (tsim.Count > 0)
{
dt = sp.T - tsim.Last();
}
else
{
dt = sp.T;
}
if (Double.IsNaN(sp.X[0]))
{
return;
}
//xsim[plotLocs.Count].Add(dt);
tsim.Add(sp.T);
for (int i = 0; i < plotLocs.Count; i++)
{
double x = 0.0;
foreach (int j in plotLocs[i])
{
x += sp.X[j];
}
xsim[i].Add(x);
}
}
}
public Complex[] NextStep (Ode ode, double t, Complex[] y)
{
Complex[] ya = new Complex[y.Length];
Complex[] yb = new Complex[y.Length];
Complex[] yc = new Complex[y.Length];
Complex[] yr = new Complex[y.Length];
Complex[] dya, dyb, dyc;
Complex[] dy = ode.Calc (t, y);
for (int i = 0; i < y.Length; i++) {
ya [i] = y [i] + .5 * _h * dy [i];
}
dya = ode.Calc (t + .5 * _h.Real, ya);
for (int i = 0; i < y.Length; i++) {
yb [i] = y [i] + .5 * _h * dya [i];
}
dyb = ode.Calc (t + .5 * _h.Real, yb);
for (int i = 0; i < y.Length; i++) {
yc [i] = y [i] + _h * dyb [i];
}
dyc = ode.Calc (t + _h.Real, yc);
double _1_6 = 1.0 / 6.0;
for (int i = 0; i < y.Length; i++) {
yr [i] = y [i] + _1_6 * _h * (dy [i] + 2 * (dya[i] + dyb[i]) + dyc[i]);
}
return yr;
}
public override Radian GetAngle()
{
if (joint.jointID == IntPtr.Zero)
{
return(0);
}
return(-Ode.dJointGetHingeAngle(joint.jointID));
}
public TimeList<Complex[]> Solve(Ode ode, Complex[] y0, double t)
{
if(_threadCount > 1) {
return CalcThreaded (ode, y0, t);
} else {
return CalcSingle (ode, y0, t);
}
}
protected override void OnSetAngularVelocity()
{
if (PushedToWorld && bodyID != IntPtr.Zero)
{
Ode.dBodySetAngularVel(bodyID, AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
//freelySpinning = false;
}
}
public Complex[] NextStep (Ode ode, double t, Complex[] y)
{
Complex[] n = ode.Calc (t, y);
for(int i = 0; i < y.Length; i++) {
n[i] = y[i] + n[i] * _h;
}
return n;
}
public SegmentCode()
{
ode = Ode.anode;
for (int i = 0; i < 8; i++)
{
dic[(char)(97 + i)] = 0;
}
}
public static double GetCylIntegrX(Func <double, double> f_ot_x, double a, double b)
{
var f2 = new Func <double, Vector, Vector>((x, v) => new Vector(Pow(f_ot_x(x), 2)));
var v0 = new Vector(0);
var sol = Ode.Adams4(a, v0, f2, (b - a) / 300).SolveTo(b);
return(PI * sol.Last().X[0]);
}
protected override RayCastResult[] OnRayCastPiercing(Ray ray, int contactGroup)
{
if (float.IsNaN(ray.Origin.X))
{
Log.Fatal("PhysicsWorld.RayCast: Single.IsNaN(ray.Origin.X)");
}
if (float.IsNaN(ray.Direction.X))
{
Log.Fatal("PhysicsWorld.RayCast: Single.IsNaN(ray.Direction.X)");
}
if (ray.Direction == Vec3.Zero)
{
return(emptyPiercingRayCastResult);
}
Vec3 dirNormal = ray.Direction;
float length = dirNormal.Normalize();
Ode.dGeomRaySet(rayCastGeomID, ray.Origin.X, ray.Origin.Y, ray.Origin.Z,
dirNormal.X, dirNormal.Y, dirNormal.Z);
Ode.dGeomRaySetLength(rayCastGeomID, length);
int count;
IntPtr data;
Ode.DoRayCastPiercing(neoAxisAdditionsID, contactGroup, out count, out data);
if (count == 0)
{
return(emptyPiercingRayCastResult);
}
RayCastResult[] array = new RayCastResult[count];
unsafe
{
Ode.RayCastResult *pointer = (Ode.RayCastResult *)data;
for (int n = 0; n < count; n++)
{
RayCastResult result = new RayCastResult();
result.Shape = shapesDictionary[pointer->shapeDictionaryIndex].shape;
result.Position = Convert.ToNet(pointer->position);
result.Normal = Convert.ToNet(pointer->normal);
result.Distance = pointer->distance;
result.TriangleID = pointer->triangleID;
array[n] = result;
pointer++;
}
}
//sort by distance
ArrayUtils.SelectionSort(array, rayCastResultDistanceComparer);
return(array);
}
void UpdateODEJoint()
{
bool needCreate = PushedToWorld && !Broken;
bool created = jointID != dJointID.Zero;
if (needCreate == created)
{
return;
}
if (needCreate)
{
if (Math.Abs(Vec3.Dot(axis1.Direction, axis2.Direction)) > .095f)
{
Log.Warning("UniversalJoint: Invalid axes.");
return;
}
if (Axis1.LimitsEnabled && Axis1.LimitLow > Axis1.LimitHigh)
{
Log.Warning("UniversalJoint: Invalid axis1 limits (low > high).");
return;
}
if (Axis2.LimitsEnabled && Axis2.LimitLow > Axis2.LimitHigh)
{
Log.Warning("UniversalJoint: Invalid axis2 limits (low > high).");
return;
}
ODEBody odeBody1 = (ODEBody)Body1;
ODEBody odeBody2 = (ODEBody)Body2;
jointID = Ode.dJointCreateUniversal(((ODEPhysicsScene)Scene).worldID, IntPtr.Zero);
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
Ode.dJointSetUniversalParam(jointID, Ode.dJointParams.dParamFudgeFactor,
Defines.jointFudgeFactor);
Ode.dJointSetUniversalParam(jointID, Ode.dJointParams.dParamFudgeFactor2,
Defines.jointFudgeFactor);
Ode.dJointAttach(jointID, odeBody1.bodyID, odeBody2.bodyID);
Ode.dJointSetUniversalAnchor(jointID, Anchor.X, Anchor.Y, Anchor.Z);
axis1.UpdateToLibrary(true);
axis2.UpdateToLibrary(true);
axis1LocalAxis = Body1.Rotation.GetInverse() * axis1.Direction;
axis2LocalAxis = Body1.Rotation.GetInverse() * axis2.Direction;
Ode.BodyDataAddJoint(odeBody1.bodyData, jointID);
Ode.BodyDataAddJoint(odeBody2.bodyData, jointID);
}
else
{
DestroyODEJoint();
}
}
public void addHeightField(Space space, double[] field, float width, float depth, int widthsamples, int depthsamples)
{
IntPtr dataID = Ode.dGeomHeightfieldDataCreate();
Ode.dGeomHeightfieldDataBuildDouble(dataID, field, 1, width, depth, widthsamples, depthsamples, 1.0f, 0.0f, 1.0f, 0);
IntPtr heightfield = Ode.dCreateHeightfield(space.getSpaceID(), dataID, 1);
Ode.dGeomSetPosition(heightfield, width / 2.0f, 0.0f, depth / 2.0f);
}
protected override void OnUpdateContactsEnabled()
{
base.OnUpdateContactsEnabled();
if (jointID != dJointID.Zero)
{
Ode.SetJointContactsEnabled(jointID, ContactsEnabled);
}
}
//internal void DoAngularVelocityFix()
//{
// if( nonSymmetricInertia )
// {
// Vec3 velocity = AngularVelocity;
// float currentAngVelMagSquared = velocity.LengthSqr();
// if( freelySpinning )
// {
// // If the current angular velocity magnitude is greater than
// // that of the previous step, scale it by that of the previous
// // step; otherwise, update the previous value to that of the
// // current step. This ensures that angular velocity never
// // increases for freely-spinning objects.
// if( currentAngVelMagSquared > prevAngVelMagSquared )
// {
// float currentAngVelMag = MathFunctions.Sqrt16( currentAngVelMagSquared );
// velocity = velocity / currentAngVelMag;
// // Vel is now a unit vector. Next, scale this vector
// // by the previous angular velocity magnitude.
// float prevAngVelMag = MathFunctions.Sqrt16( prevAngVelMagSquared );
// AngularVelocity = velocity * prevAngVelMag;
// }
// }
// prevAngVelMagSquared = currentAngVelMagSquared;
// }
// // Reset the "freely-spinning" parameter for the next time step.
// freelySpinning = true;
// //fix MaxAngularVelocity
// {
// float max = ODEPhysicsWorld.Instance.MaxAngularVelocity;
// Vec3 vel = AngularVelocity;
// bool changed = false;
// if( vel.X < -max ) { vel.X = -max; changed = true; }
// else if( vel.X > max ) { vel.X = max; changed = true; }
// if( vel.Y < -max ) { vel.Y = -max; changed = true; }
// else if( vel.Y > max ) { vel.Y = max; changed = true; }
// if( vel.Z < -max ) { vel.Z = -max; changed = true; }
// else if( vel.Z > max ) { vel.Z = max; changed = true; }
// if( changed )
// AngularVelocity = vel;
// }
//}
public override void ClearForces()
{
base.ClearForces();
if (bodyID != dBodyID.Zero)
{
Ode.dBodySetForce(bodyID, 0, 0, 0);
Ode.dBodySetTorque(bodyID, 0, 0, 0);
}
}
void UpdateSuspension()
{
if (suspensionInitialized)
{
Ode.dJointSetHinge2Param(jointID, Ode.dJointParams.dParamSuspensionCFM,
suspensionCFM);
Ode.dJointSetHinge2Param(jointID, Ode.dJointParams.dParamSuspensionERP,
suspensionERP);
}
}
internal static void FreeJointFeedback(dJointID jointID)
{
IntPtr jointFeedback = Ode.dJointGetFeedbackAsIntPtr(jointID);
if (jointFeedback != IntPtr.Zero)
{
Ode.dFree(jointFeedback, (uint)Marshal.SizeOf(typeof(Ode.dJointFeedback)));
jointFeedback = IntPtr.Zero;
}
}
public static warp_Quaternion Ode2WarpQuaternion( Ode.Quaternion odeQ )
{
warp_Quaternion q = new warp_Quaternion(
( float )odeQ.X,
( float )odeQ.Y,
( float )odeQ.Z,
( float )odeQ.W );
return q;
}
public override void SetDesiredVelocity(Radian velocity, float maxTorque)
{
if (joint.jointID == dJointID.Zero)
{
return;
}
Ode.dJointSetHingeParam(joint.jointID, Ode.dJointParams.dParamVel,
joint.Body1.Static ? velocity : -velocity);
Ode.dJointSetHingeParam(joint.jointID, Ode.dJointParams.dParamFMax, maxTorque);
}
protected override void OnShapeSetMaterialProperty(Shape shape)
{
GeomData geomData = GetGeomDataByShape(shape);
if (geomData != null)
{
dGeomID geomID = (geomData.transformID != dGeomID.Zero) ? geomData.transformID : geomData.geomID;
Ode.SetShapeMaterialProperties(geomID, shape.Hardness, shape.Restitution, shape.DynamicFriction,
shape.StaticFriction);
}
}
public void ExponentSolveToMidPointTest1()
{
foreach (var sp in Ode.MidPoint(0,
1,
(t, x) => - x,
new Options {
RelativeTolerance = 1e-3, InitialStep = 0.1
}).SolveTo(1000))
{
Assert.IsTrue(Math.Abs(sp.X[0] - Math.Exp(-sp.T)) < 1e-2);
}
}
public SolPoint[] calcSIR()
{
var sol = Ode.RK547M(
0,
new Vector(SuseptipleFaction, InfectedFaction),
(t, x) => new Vector(
(1 - p) * f - r0 * x[0] * x[1] - f * x[0],
r0 * x[0] * x[1] - x[1]));
var points = sol.SolveFromToStep(0, TimeLimit, Step).ToArray();
return(points);
}
private void initObs(OneDemExample calc)
{
pr = calc;
v0 = pr.Rebuild(pr.TimeSynch);
sol = Ode.RK45(pr.TimeSynch, v0, pr.f, pr.dt).WithStepRx(0.01, out controller);
controller.Pause();
sol.ObserveOnDispatcher().Subscribe(sp => {
vm.SolPointList.Update(sp);
slider.Maximum = (double)(vm.SolPointList.Value.Count > 0 ? vm.SolPointList.Value.Count : 0);
});
}
public void ExponentSolveToGEarTest()
{
foreach (var sp in Ode.GearBDF(0,
1,
(t, x) => - x,
new Options {
RelativeTolerance = 1e-4
}).SolveTo(10))
{
Assert.IsTrue(Math.Abs(sp.X[0] - Math.Exp(-sp.T)) < 1e-3);
}
}
public void ExponentSolveToRKTest()
{
foreach (var sp in Ode.RK547M(0,
1,
(t, x) => - x,
new Options {
RelativeTolerance = 1e-3
}).SolveTo(1000))
{
Assert.IsTrue(Math.Abs(sp.X[0] - Math.Exp(-sp.T)) < 1e-2);
}
}
protected override Body[] OnVolumeCast(Sphere sphere, int contactGroup)
{
dGeomID volumeCastGeomID = Ode.dCreateSphere(rootSpaceID, sphere.Radius);
Ode.dGeomSetPosition(volumeCastGeomID, sphere.Origin.X, sphere.Origin.Y, sphere.Origin.Z);
Body[] result = DoVolumeCastGeneral(volumeCastGeomID, contactGroup);
Ode.dGeomDestroy(volumeCastGeomID);
return(result);
}
private TimeList<Complex[]> CalcSingle(Ode ode, Complex[] y0, double t_max)
{
TimeList<Complex[]> solve = new TimeList<Complex[]> ();
Complex[] y = y0;
for(double t = 0; t < t_max; t += _step.Stepsize) {
y = _step.NextStep (ode, t, y);
solve.Add (t, y);
}
return solve;
}
public void KernelTest3()
{
double h = 1;
Func <double, Vector, Vector> tstf = (t, v) => {
var res = Vector.Zeros(1);
res[0] = KernelF.W(t, h);
return(res);
};
var sol = Ode.RK45(-3 * h, Vector.Zeros(1), tstf, 0.01).SolveFromTo(-2.5 * h, 2.5 * h).Last();
Assert.AreEqual(1d, sol.X[0], 0.0001);
}
protected override void OnShapeSetContactGroup(Shape shape)
{
base.OnShapeSetContactGroup(shape);
GeomData geomData = GetGeomDataByShape(shape);
if (geomData != null)
{
dGeomID geomID = (geomData.transformID != dGeomID.Zero) ?
geomData.transformID : geomData.geomID;
Ode.SetShapeContractGroup(geomID, shape.ContactGroup);
}
}
public void ExponentSolveToArrayTest()
{
var arr = Ode.RK547M(0,
1,
(t, x) => - x,
new Options {
RelativeTolerance = 1e-3
}).SolveTo(1000).ToArray();
foreach (var sp in arr)
{
Assert.IsTrue(Math.Abs(sp.X[0] - Math.Exp(-sp.T)) < 1e-2); // AbsTol instead of 1e-4
}
}
static int Main()
{
double a = 0;
vector ya = new vector(0, 1);
double b = 2.25 * PI;
double h = 0.1, acc = 1e-3, eps = 1e-3;
int max = 999;
var result = Ode.driver(F, a, ya, b, h, acc: acc, eps: eps, maxsteps: max);
File.WriteAllLines("plotA.txt", result.xs
.Select((x, i) => $"{x} {result.ys[i][0]} {result.ys[i][1]}"));
return(0);
}
internal void UpdateDataFromLibrary()
{
if (jointID == dJointID.Zero)
{
return;
}
Ode.dVector3 odeAxisDirection = new Ode.dVector3();
Ode.dJointGetSliderAxis(jointID, ref odeAxisDirection);
Vec3 axisDirection = Convert.ToNet(odeAxisDirection);
UpdateDataFromLibrary(ref axisDirection);
}
public IEnumerable <SolPoint> Solve()
{
var sol = Ode.RK547M(
0,
new Vector(Ag, D, Ab),
(t, x) => new Vector(
b * x[0] - y * x[2] * x[0],
a * x[0] - mc * x[1],
p * x[1] - n * x[0] * x[2] - mf * x[2]
)
);
return(sol.SolveFromToStep(0, 100, 1));
}
void PushToWorld()
{
if (!Static)
{
bodyID = Ode.dBodyCreate(scene.worldID);
UpdateSleepiness();
Ode.dBodySetPosition(bodyID, Position.X, Position.Y, Position.Z);
Ode.dQuaternion odeQuat;
Convert.ToODE(Rotation, out odeQuat);
Ode.dBodySetQuaternion(bodyID, ref odeQuat);
Ode.dBodySetLinearVel(bodyID, LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
Ode.dBodySetAngularVel(bodyID, AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
if (Sleeping)
{
Ode.dBodyDisable(bodyID);
}
else
{
Ode.dBodyEnable(bodyID);
}
if (!EnableGravity)
{
Ode.dBodySetGravityMode(bodyID, 0);
}
}
bodyData = Ode.CreateBodyData(bodyID);
CreateGeomDatas();
//no shapes
if (geomDatas == null || geomDatas.Length == 0)
{
PopFromWorld();
return;
}
if (!Static)
{
CalculateBodyMass();
}
RecreateAttachedJoints();
}
public static warp_Matrix Ode2WarpMatrix( Ode.Matrix3 m )
{
warp_Matrix result = new warp_Matrix();
result.m11 = ( float )m.m11;
result.m12 = ( float )m.m12;
result.m13 = ( float )m.m13;
result.m21 = ( float )m.m21;
result.m22 = ( float )m.m22;
result.m23 = ( float )m.m23;
result.m31 = ( float )m.m31;
result.m32 = ( float )m.m32;
result.m33 = ( float )m.m33;
return result;
}
private TimeList<Complex[]> CalcThreaded(Ode ode, Complex[] y0, double t_max)
{
int ptr = 0;
int N = ode.N;
double t = 0;
TimeList<Complex[]> solve = new TimeList<Complex[]> ();
Complex[] y = new Complex[N];
Complex[] lastY = y0;
for (int thread = 0; thread < _threadCount; thread++) {
ThreadPool.QueueUserWorkItem (new WaitCallback (delegate(object data) {
int c;
while (t < t_max) {
while ((c = Interlocked.Increment(ref ptr)) <= N) {
y[c - 1] = _step.NextStepComponent(c - 1, ode, t, lastY);
}
lock(syncLock) {
((AutoResetEvent)data).Set();
Monitor.Wait(syncLock);
}
}
}), _resets [thread]);
}
for (t = 0; t < t_max; t += _step.Stepsize) {
WaitHandle.WaitAll (_resets);
lock(syncLock) {
ptr = 0;
solve.Add(t, y);
lastY = y;
y = new Complex[N];
Monitor.PulseAll (syncLock);
}
}
return solve;
}
public static extern void SetViewpoint(ref Ode.Vector3 xyz, ref Ode.Vector3 hpr);
public Complex NextStepComponent (int j, Ode ode, double t, Complex[] y)
{
return y [j] + ode.CalcComponent (j, t, y) * _h;
}
public static extern void DrawSphere(ref Ode.Vector3 pos, ref Ode.Matrix3 R, dReal radius);
public void SolveToFile(Ode ode, Complex[] y0, double t, string fileName)
{
throw new NotImplementedException ();
}
public static extern void DrawCylinder(ref Ode.Vector3 pos, ref Ode.Matrix3 R, dReal length, dReal radius);
public TimeList<Complex[]> Solve(Ode ode, Complex[] y0)
{
throw new NotImplementedException ();
}
public static warp_Vector Ode2DxVector3( Ode.Vector3 odeV )
{
return new warp_Vector( ( float )odeV.X, ( float )odeV.Y, ( float )odeV.Z );
}
protected virtual void CollideCallback( Ode.Geom o1, Ode.Geom o2 )
{
ContactGeom[] cgeoms = o1.Collide( o2, 30 );
if ( cgeoms.Length > 0 )
{
Contact[] contacts = Contact.FromContactGeomArray( cgeoms );
for ( int i = 0; i < contacts.Length; i++ )
{
contacts[ i ].Surface.mode = SurfaceMode.Bounce;
contacts[ i ].Surface.mu = 100; //World.Infinity;
contacts[ i ].Surface.bounce = 0.70f;
contacts[ i ].Surface.bounce_vel = 0.05f;
ContactJoint cj = new ContactJoint( _world, _contactGroup, contacts[ i ] );
cj.Attach( contacts[ i ].Geom.Geom1.Body, contacts[ i ].Geom.Geom2.Body );
}
}
}
/// <summary>
/// オブジェクト関節のパラメータを取得
/// </summary>
/// <param name="parameter">取得するパラメータ</param>
/// <returns>パラメータ値</returns>
public Real GetJointParam(Ode.JointParam parameter)
{
switch (_jointType)
{
case Ode.JointType.Hinge:
return Ode.JointGetHingeParam(_joint, (int)parameter);
case Ode.JointType.Slider:
return Ode.JointGetSliderParam(_joint, (int)parameter);
default:
return 0;
}
}
/// <summary>
/// オブジェクト関節のパラメータを設定
/// </summary>
/// <param name="parameter">設定を行うパラメータ</param>
/// <param name="value">パラメータ値</param>
public void SetJointParam(Ode.JointParam parameter, Real value)
{
switch (_jointType)
{
case Ode.JointType.Hinge:
Ode.JointSetHingeParam(_joint, (int)parameter, value);
break;
case Ode.JointType.Slider:
Ode.JointSetSliderParam(_joint, (int)parameter, value);
break;
default:
break;
}
}
/// <summary>
/// オブジェクト関節を生成する
/// <param name="type">生成する関節の種類</param>
/// <param name="attachObject">関節を接続するオブジェクト</param>
/// </summary>
public void CreateJoint(Ode.JointType type, ObjectSettings attachObject = null)
{
switch (type)
{
// 固定関節
case Ode.JointType.Fixed:
_joint = Ode.JointCreateFixed(_worldID, JointGroupID.Zero); // 固定関節（土台と地面の固定）
Ode.JointAttach(_joint, _body, JointGroupID.Zero); // 固定関節の取付け
Ode.JointSetFixed(_joint); // 固定関節の設定
_jointType = Ode.JointType.Fixed;
break;
// ヒンジ関節
case Ode.JointType.Hinge:
_joint = Ode.JointCreateHinge(_worldID, JointGroupID.Zero); // ヒンジ関節生成
Ode.JointAttach(_joint, attachObject._body , _body); // 関節を取り付ける
Ode.JointSetHingeAnchor(_joint, _jointRotCenter.X, _jointRotCenter.Y, _jointRotCenter.Z); // 関節中心の設定
Ode.JointSetHingeAxis(_joint, _jointRotAxis.X, _jointRotAxis.Y, _jointRotAxis.Z); // 関節回転軸の設定
_jointType = Ode.JointType.Hinge;
break;
// スライダー関節
case Ode.JointType.Slider:
_joint = Ode.JointCreateSlider(_worldID, JointGroupID.Zero);// スライダー関節生成
Ode.JointAttach(_joint, attachObject._body , _body); // 関節を取り付ける
Ode.JointSetSliderAxis(_joint, _jointRotAxis.X, _jointRotAxis.Y, _jointRotAxis.Z); // 関節スライド軸の設定
_jointType = Ode.JointType.Slider;
break;
default:
break;
}
}
// internal void waitForSpaceUnlock(IntPtr space)
// {
// //if (space != IntPtr.Zero)
// //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
// }
// /// <summary>
// /// Debug space message for printing the space that a prim/avatar is in.
// /// </summary>
// /// <param name="pos"></param>
// /// <returns>Returns which split up space the given position is in.</returns>
// public string whichspaceamIin(Vector3 pos)
// {
// return calculateSpaceForGeom(pos).ToString();
// }
#region Collision Detection
/// <summary>
/// Collides two geometries.
/// </summary>
/// <returns></returns>
/// <param name='geom1'></param>
/// <param name='geom2'>/param>
/// <param name='maxContacts'></param>
/// <param name='contactsArray'></param>
/// <param name='contactGeomSize'></param>
private int CollideGeoms(
IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize)
{
int count;
lock (OdeScene.UniversalColliderSyncObject)
{
// We do this inside the lock so that we don't count any delay in acquiring it
if (CollectStats)
m_nativeCollisionStartTick = Util.EnvironmentTickCount();
count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize);
}
// We do this outside the lock so that any waiting threads aren't held up, though the effect is probably
// negligable
if (CollectStats)
m_stats[ODENativeGeomCollisionFrameMsStatName]
+= Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
return count;
}
public static extern void DrawBox(ref Ode.Vector3 pos, ref Ode.Matrix3 R, ref Ode.Vector3 sides);
/// <summary>
/// 位置行列・回転行列を取得
/// </summary>
/// <param name="pos">位置行列</param>
/// <param name="R">回転行列</param>
public void GetStatus(out Ode.Vector3 pos, out Ode.Matrix3 R)
{
pos = _carBody.GetBodyPosition();
R = _carBody.GetBodyRotation();
}
public static extern void DrawConvex(ref Ode.Vector3 pos, ref Ode.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
