public Vec4d TransformVector(Vec4d vec)
{
Vec4d outval = new Vec4d();
Mat4f.MulWithVec4(Values, vec, outval);
return(outval);
}
public static Point3d TransformPoint3d(this Mat pose, Point3d point)
{
if (pose.Cols != 4 || pose.Rows != 4)
{
throw new ArgumentException("Matrix must be a 4 x 4 matrix");
}
if (pose.Type() != MatType.CV_64FC1)
{
throw new ArgumentException("Matrix must be of float 32 bit type");
}
var m1 = new float[4, 4];
var data = new float[16];
pose.GetArray(out data);
Array.Copy(data, m1, data.Length);
var v1 = new double[4] {
point.X, point.Y, point.Z, 1
};
var homogenous = new Vec4d(
(m1[0, 0] * v1[0] + m1[0, 1] * v1[1] + m1[0, 2] * v1[2] + m1[0, 3] * v1[3]),
(m1[1, 0] * v1[0] + m1[1, 1] * v1[1] + m1[1, 2] * v1[2] + m1[1, 3] * v1[3]),
(m1[2, 0] * v1[0] + m1[2, 1] * v1[1] + m1[2, 2] * v1[2] + m1[2, 3] * v1[3]),
(m1[3, 0] * v1[0] + m1[3, 1] * v1[1] + m1[3, 2] * v1[2] + m1[3, 3] * v1[3])
);
return(new Point3d(homogenous.Item0, homogenous.Item1, homogenous.Item2));
}
public bool OnInteract(IPlayer byPlayer, BlockSelection blockSel)
{
ItemSlot handslot = byPlayer.InventoryManager.ActiveHotbarSlot;
Vec4d vec = new Vec4d(blockSel.HitPosition.X, blockSel.HitPosition.Y, blockSel.HitPosition.Z, 1);
Vec4d tvec = mat.TransformVector(vec);
int a = Facing.Axis == EnumAxis.Z ? 1 : 0;
ItemSlot targetSlot = tvec.X < 0.5 ? inv[a] : inv[1 - a];
if (handslot.Empty)
{
TryTake(targetSlot, byPlayer);
}
else
{
if (TryAddPart(handslot, byPlayer))
{
return(true);
}
if (handslot.Itemstack.Collectible.CrushingProps != null)
{
TryPut(handslot, targetSlot);
}
}
return(true);
}
public bool OnInteract(IPlayer byPlayer, BlockSelection blockSel)
{
ItemSlot handslot = byPlayer.InventoryManager.ActiveHotbarSlot;
Vec4d vec = new Vec4d(blockSel.HitPosition.X, blockSel.HitPosition.Y, blockSel.HitPosition.Z, 1);
Vec4d tvec = mat.TransformVector(vec);
int a = Facing.Axis == EnumAxis.Z ? 1 : 0;
ItemSlot targetSlot = tvec.X < 0.5 ? inv[a] : inv[1 - a];
if (handslot.Empty)
{
TryTake(targetSlot, byPlayer);
}
else
{
if (TryAddPart(handslot, byPlayer))
{
var pos = Pos.ToVec3d().Add(0.5, 0.25, 0.5);
Api.World.PlaySoundAt(Block.Sounds.Place, pos.X, pos.Y, pos.Z, byPlayer);
(Api as ICoreClientAPI)?.World.Player.TriggerFpAnimation(EnumHandInteract.HeldItemInteract);
return(true);
}
if (handslot.Itemstack.Collectible.CrushingProps != null)
{
TryPut(handslot, targetSlot);
}
}
return(true);
}
static void Main(string[] args)
{
Vec4d vector1 = new Vec4d(1.0, 2.0, 3.0, 4.0);
Vec4d vector2 = new Vec4d(1.0, 2.0, 3.0, 4.0);
Console.WriteLine(vector1.Item0);
Console.WriteLine(vector1[1]);
Console.WriteLine(vector1.Equals(vector2));
}
/// <summary>
///
/// </summary>
/// <returns></returns>
public static Vec4d Apply(this Transform xform, Vec4d vector)
{
return(new Vec4d(
vector.X * xform.M00 + vector.Y * xform.M01 + vector.Z * xform.M02 + vector.W * xform.M03,
vector.X * xform.M10 + vector.Y * xform.M11 + vector.Z * xform.M12 + vector.W * xform.M13,
vector.X * xform.M20 + vector.Y * xform.M21 + vector.Z * xform.M22 + vector.W * xform.M23,
vector.W
));
}
/// <summary>
/// Multiply the matrix with a vec4. Reference: http://mathinsight.org/matrix_vector_multiplication
///
/// </summary>
/// <param name="matrix"></param>
/// <param name="inVal"></param>
/// <param name="outVal"></param>
/// <returns></returns>
public static void MulWithVec4(double[] matrix, Vec4d inVal, Vec4d outVal)
{
outVal.Set(0, 0, 0, 0);
for (int row = 0; row < 4; row++)
{
for (int col = 0; col < 4; col++)
{
outVal[row] += matrix[4 * col + row] * inVal[col];
}
}
}
public void WriteFrames4d(BinaryWriter writer, byte frame_t, SortedList <int, object> frames)
{
WriteFrameT(writer, frame_t, frames.Count);
foreach (KeyValuePair <int, object> pair in frames)
{
Vec4d value = (Vec4d)pair.Value;
WriteFrameT(writer, frame_t, pair.Key);
writer.Write(value.x);
writer.Write(value.y);
writer.Write(value.z);
writer.Write(value.w);
}
}
public static Vec4d UnpackRotation(ushort a, ushort b, ushort c)
{
Vec4d q = new Vec4d();
int axis1 = a >> 15;
int axis2 = b >> 15;
int axis = axis2 << 1 | axis1;
a = (ushort)(a & 0x7FFF);
b = (ushort)(b & 0x7FFF);
double x, y, z, w;
x = 1.41421 * (a - 0x4000) / 0x8000;
y = 1.41421 * (b - 0x4000) / 0x8000;
z = 1.41421 * (c - 0x8000) / 0x10000;
w = Math.Pow(1.0 - x * x - y * y - z * z, 0.5);
// Console.Out.WriteLine("Unpack Values: X: {0}, Y: {1}, Z: {2}, W: {3}, Axis: {4}", x, y, z, w, axis);
if (axis == 0)
{
q = new Vec4d(w, x, y, z);
}
else if (axis == 1)
{
q = new Vec4d(x, w, y, z);
}
else if (axis == 2)
{
q = new Vec4d(x, y, w, z);
}
else if (axis == 3)
{
q = new Vec4d(x, y, z, w);
}
else
{
Console.Out.WriteLine("Unknown Axis detected! Axis: %s", axis);
}
return(q);
}
public Animation(Stream animStream, bool leaveOpen = true, bool alwaysAddBones = false)
{
Animations = new List <Keyframe>();
// Convert OW Animation to our Animation Type
using (BinaryReader animReader = new BinaryReader(animStream, Encoding.Default, leaveOpen)) {
Header = animReader.Read <AnimHeader>();
Duration = Header.duration;
FramesPerSecond = Header.fps;
InfoTableSize = (int)(Header.fps * Header.duration) + 1;
ushort bonecount = Header.bonecount;
animStream.Seek((long)Header.boneListOffset, SeekOrigin.Begin);
for (uint i = 0; i < Header.bonecount; i++)
{
int boneID = animReader.ReadInt32();
BoneList.Add(boneID);
}
Vec3d[,] ScaleValues = new Vec3d[bonecount, InfoTableSize];
Vec3d[,] PositionValues = new Vec3d[bonecount, InfoTableSize];
Vec4d[,] RotationValues = new Vec4d[bonecount, InfoTableSize];
bool[,] hasScale = new bool[bonecount, InfoTableSize];
bool[,] hasPosition = new bool[bonecount, InfoTableSize];
bool[,] hasRotation = new bool[bonecount, InfoTableSize];
animStream.Seek((long)Header.infoTableOffset, SeekOrigin.Begin);
for (int boneid = 0; boneid < Header.bonecount; boneid++)
{
long animStreamPos = animStream.Position;
AnimInfoTable it = animReader.Read <AnimInfoTable>();
long SIO = (long)it.ScaleIndicesOffset * 4 + animStreamPos;
long PIO = (long)it.PositionIndicesOffset * 4 + animStreamPos;
long RIO = (long)it.RotationIndicesOffset * 4 + animStreamPos;
long SDO = (long)it.ScaleDataOffset * 4 + animStreamPos;
long PDO = (long)it.PositionDataOffset * 4 + animStreamPos;
long RDO = (long)it.RotationDataOffset * 4 + animStreamPos;
InfoTables.Add(it);
// Read Indices
List <int> ScaleIndexList = new List <int>();
animStream.Seek(SIO, SeekOrigin.Begin);
for (int j = 0; j < it.ScaleCount; j++)
{
if (InfoTableSize <= 255)
{
ScaleIndexList.Add((int)animReader.ReadByte());
}
else
{
ScaleIndexList.Add((int)animReader.ReadInt16());
}
}
List <int> PositonIndexList = new List <int>();
animStream.Seek(PIO, SeekOrigin.Begin);
for (int j = 0; j < it.PositionCount; j++)
{
if (InfoTableSize <= 255)
{
PositonIndexList.Add((int)animReader.ReadByte());
}
else
{
PositonIndexList.Add((int)animReader.ReadInt16());
}
}
List <int> RotationIndexList = new List <int>();
animStream.Seek(RIO, SeekOrigin.Begin);
for (int j = 0; j < it.RotationCount; j++)
{
if (InfoTableSize <= 255)
{
RotationIndexList.Add((int)animReader.ReadByte());
}
else
{
RotationIndexList.Add((int)animReader.ReadInt16());
}
}
// Read Data
animStream.Seek(SDO, SeekOrigin.Begin);
for (int j = 0; j < it.ScaleCount; j++)
{
int Index = Math.Abs(ScaleIndexList[j]) % InfoTableSize;
hasScale[boneid, Index] = true;
ushort x = animReader.ReadUInt16();
ushort y = animReader.ReadUInt16();
ushort z = animReader.ReadUInt16();
Vec3d values = UnpackScale(x, y, z);
ScaleValues[boneid, Index] = values;
}
animStream.Seek(PDO, SeekOrigin.Begin);
for (int j = 0; j < it.PositionCount; j++)
{
int Index = Math.Abs(PositonIndexList[j]) % InfoTableSize;
hasPosition[boneid, Index] = true;
float x = animReader.ReadSingle();
float y = animReader.ReadSingle();
float z = animReader.ReadSingle();
Vec3d values = new Vec3d(x, y, z);
PositionValues[boneid, Index] = values;
}
animStream.Seek(RDO, SeekOrigin.Begin);
for (int j = 0; j < it.RotationCount; j++)
{
int Index = Math.Abs(RotationIndexList[j]) % InfoTableSize;
ushort x = animReader.ReadUInt16();
ushort y = animReader.ReadUInt16();
ushort z = animReader.ReadUInt16();
Vec4d values = UnpackRotation(x, y, z);
hasRotation[boneid, Index] = true;
RotationValues[boneid, Index] = values;
}
animStream.Seek(animStreamPos + 32L, SeekOrigin.Begin);
}
for (int frame = 0; frame < InfoTableSize; frame++)
{
Keyframe kf = new Keyframe();
kf.FramePosition = ((float)frame / FramesPerSecond);
kf.FramePositionI = frame;
kf.BoneFrames = new List <BoneAnimation>();
for (int bone = 0; bone < Header.bonecount; bone++)
{
// Build Value Data
BoneAnimation ba = new BoneAnimation();
ba.BoneID = BoneList[bone];
ba.Values = new List <FrameValue>();
if (hasScale[bone, frame])
{
Vec3d v = ScaleValues[bone, frame];
FrameValue fv = new FrameValue(AnimChannelID.SCALE, v);
ba.Values.Add(fv);
}
if (hasPosition[bone, frame])
{
Vec3d v = PositionValues[bone, frame];
FrameValue f = new FrameValue(AnimChannelID.POSITION, v);
ba.Values.Add(f);
}
if (hasRotation[bone, frame])
{
Vec4d v = RotationValues[bone, frame];
FrameValue f = new FrameValue(AnimChannelID.ROTATION, v);
ba.Values.Add(f);
}
if (ba.Values.Count > 0 || alwaysAddBones)
{
kf.BoneFrames.Add(ba);
}
}
if (kf.BoneFrames.Count > 0)
{
Animations.Add(kf);
}
}
}
}
/// <summary>
/// Reconstructs points by triangulation.
/// </summary>
/// <param name="projMatr1">3x4 projection matrix of the first camera.</param>
/// <param name="projMatr2">3x4 projection matrix of the second camera.</param>
/// <param name="projPoints1">2xN array of feature points in the first image. In case of c++ version
/// it can be also a vector of feature points or two-channel matrix of size 1xN or Nx1.</param>
/// <param name="projPoints2">2xN array of corresponding points in the second image. In case of c++ version
/// it can be also a vector of feature points or two-channel matrix of size 1xN or Nx1.</param>
/// <returns>4xN array of reconstructed points in homogeneous coordinates.</returns>
public static Vec4d[] TriangulatePoints(
double[,] projMatr1, double[,] projMatr2,
IEnumerable<Point2d> projPoints1, IEnumerable<Point2d> projPoints2)
{
if (projMatr1 == null)
throw new ArgumentNullException("projMatr1");
if (projMatr2 == null)
throw new ArgumentNullException("projMatr2");
if (projPoints1 == null)
throw new ArgumentNullException("projPoints1");
if (projPoints2 == null)
throw new ArgumentNullException("projPoints2");
if (projMatr1.GetLength(0) != 3 && projMatr1.GetLength(1) != 4)
throw new ArgumentException("projMatr1 != double[3,4]");
if (projMatr2.GetLength(0) != 3 && projMatr2.GetLength(1) != 4)
throw new ArgumentException("projMatr2 != double[3,4]");
Point2d[] projPoints1Array = EnumerableEx.ToArray(projPoints1);
Point2d[] projPoints2Array = EnumerableEx.ToArray(projPoints2);
var points4D = new Vec4d[projPoints1Array.Length];
NativeMethods.calib3d_triangulatePoints_array(
projMatr1, projMatr2,
projPoints1Array, projPoints1Array.Length,
projPoints2Array, projPoints2Array.Length,
points4D);
return points4D;
}
public static extern ExceptionStatus core_Mat_push_back_Vec4d(IntPtr self, Vec4d v);
// Requirements:
// - ✔ Try to not move a lot vertically
// - ✔ If territorial: Stay close to the spawn point
// - ✔ If air habitat: Don't go above maxHeight blocks above surface
// - ✔ If land habitat: Don't walk into water, prefer surface
// - ~~If cave habitat: Prefer caves~~
// - ✔ If water habitat: Don't walk onto land
// - ✔ Try not to fall from very large heights. Try not to fall from any large heights if entity has FallDamage
// - ✔ Prefer preferredLightLevel
// - ✔ If land habitat: Must be above a block the entity can stand on
// - ✔ if failed searches is high, reduce wander range
public Vec3d loadNextWanderTarget()
{
EnumHabitat habitat = entity.Properties.Habitat;
bool canFallDamage = entity.Properties.FallDamage;
bool territorial = StayCloseToSpawn;
int tries = 9;
Vec4d bestTarget = null;
Vec4d curTarget = new Vec4d();
BlockPos tmpPos = new BlockPos();
if (FailedConsecutivePathfinds > 10)
{
WanderRangeMul = Math.Max(0.1f, WanderRangeMul * 0.9f);
}
else
{
WanderRangeMul = Math.Min(1, WanderRangeMul * 1.1f);
if (rand.NextDouble() < 0.05)
{
WanderRangeMul = Math.Min(1, WanderRangeMul * 1.5f);
}
}
float wRangeMul = WanderRangeMul;
double dx, dy, dz;
if (rand.NextDouble() < 0.05)
{
wRangeMul *= 3;
}
while (tries-- > 0)
{
dx = wanderRangeHorizontal.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
dy = wanderRangeVertical.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
dz = wanderRangeHorizontal.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
curTarget.X = entity.ServerPos.X + dx;
curTarget.Y = entity.ServerPos.Y + dy;
curTarget.Z = entity.ServerPos.Z + dz;
curTarget.W = 1;
if (StayCloseToSpawn)
{
double distToEdge = curTarget.SquareDistanceTo(SpawnPosition) / (MaxDistanceToSpawn * MaxDistanceToSpawn);
// Prefer staying close to spawn
curTarget.W = 1 - distToEdge;
}
Block block;
switch (habitat)
{
case EnumHabitat.Air:
int rainMapY = world.BlockAccessor.GetRainMapHeightAt((int)curTarget.X, (int)curTarget.Z);
// Don't fly above max height
curTarget.Y = Math.Min(curTarget.Y, rainMapY + maxHeight);
// Cannot be in water
block = entity.World.BlockAccessor.GetBlock((int)curTarget.X, (int)curTarget.Y, (int)curTarget.Z);
if (block.IsLiquid())
{
curTarget.W = 0;
}
break;
case EnumHabitat.Land:
curTarget.Y = moveDownToFloor((int)curTarget.X, curTarget.Y, (int)curTarget.Z);
// No floor found
if (curTarget.Y < 0)
{
curTarget.W = 0;
}
else
{
// Does not like water
block = entity.World.BlockAccessor.GetBlock((int)curTarget.X, (int)curTarget.Y, (int)curTarget.Z);
if (block.IsLiquid())
{
curTarget.W /= 2;
}
// Lets make a straight line plot to see if we would fall off a cliff
bool stop = false;
bool willFall = false;
float angleHor = (float)Math.Atan2(dx, dz) + GameMath.PIHALF;
Vec3d target1BlockAhead = curTarget.XYZ.Ahead(1, 0, angleHor);
Vec3d startAhead = entity.ServerPos.XYZ.Ahead(1, 0, angleHor); // Otherwise they are forever stuck if they stand over the edge
int prevY = (int)startAhead.Y;
GameMath.BresenHamPlotLine2d((int)startAhead.X, (int)startAhead.Z, (int)target1BlockAhead.X, (int)target1BlockAhead.Z, (x, z) =>
{
if (stop)
{
return;
}
double nowY = moveDownToFloor(x, prevY, z);
// Not more than 4 blocks down
if (nowY < 0 || prevY - nowY > 4)
{
willFall = true;
stop = true;
}
// Not more than 2 blocks up
if (nowY - prevY > 2)
{
stop = true;
}
prevY = (int)nowY;
});
if (willFall)
{
curTarget.W = 0;
}
}
break;
case EnumHabitat.Sea:
block = entity.World.BlockAccessor.GetBlock((int)curTarget.X, (int)curTarget.Y, (int)curTarget.Z);
if (!block.IsLiquid())
{
curTarget.W = 0;
}
break;
}
if (curTarget.W > 0)
{
// Try to not hug the wall so much
for (int i = 0; i < BlockFacing.HORIZONTALS.Length; i++)
{
BlockFacing face = BlockFacing.HORIZONTALS[i];
block = entity.World.BlockAccessor.GetBlock((int)curTarget.X + face.Normali.X, (int)curTarget.Y, (int)curTarget.Z + face.Normali.Z);
if (block.SideSolid[face.Opposite.Index])
{
curTarget.W *= 0.5;
}
}
}
if (preferredLightLevel != null)
{
tmpPos.Set((int)curTarget.X, (int)curTarget.Y, (int)curTarget.Z);
int lightdiff = Math.Abs((int)preferredLightLevel - entity.World.BlockAccessor.GetLightLevel(tmpPos, EnumLightLevelType.MaxLight));
curTarget.W /= Math.Max(1, lightdiff);
}
if (bestTarget == null || curTarget.W > bestTarget.W)
{
bestTarget = new Vec4d(curTarget.X, curTarget.Y, curTarget.Z, curTarget.W);
}
}
if (bestTarget.W > 0)
{
//double bla = bestTarget.Y;
//bestTarget.Y += 1;
//dx = bestTarget.X - entity.ServerPos.X;
//dz = bestTarget.Z - entity.ServerPos.Z;
//Vec3d sadf = bestTarget.XYZ.Ahead(1, 0, (float)Math.Atan2(dx, dz) + GameMath.PIHALF);
/*(entity.Api as ICoreServerAPI).World.HighlightBlocks(world.AllOnlinePlayers[0], 10, new List<BlockPos>() {
* new BlockPos((int)bestTarget.X, (int)bestTarget.Y, (int)bestTarget.Z) }, new List<int>() { ColorUtil.ColorFromRgba(0, 255, 0, 80) }, EnumHighlightBlocksMode.Absolute, EnumHighlightShape.Arbitrary);
* (entity.Api as ICoreServerAPI).World.HighlightBlocks(world.AllOnlinePlayers[0], 11, new List<BlockPos>() {
* new BlockPos((int)sadf.X, (int)sadf.Y, (int)sadf.Z) }, new List<int>() { ColorUtil.ColorFromRgba(0, 255, 255, 180) }, EnumHighlightBlocksMode.Absolute, EnumHighlightShape.Arbitrary);*/
//bestTarget.Y = bla;
FailedConsecutivePathfinds = Math.Max(FailedConsecutivePathfinds - 3, 0);
return(bestTarget.XYZ);
}
FailedConsecutivePathfinds++;
return(null);
}
public static extern ExceptionStatus core_FileNode_read_Vec4d(IntPtr node, out Vec4d returnValue);
public Vec3d loadNextWanderTarget()
{
int tries = 9;
Vec4d bestTarget = null;
Vec4d curTarget = new Vec4d();
BlockPos tmpPos = new BlockPos();
if (FailedConsecutivePathfinds > 10)
{
WanderRangeMul = Math.Max(0.1f, WanderRangeMul * 0.9f);
}
else
{
WanderRangeMul = Math.Min(1, WanderRangeMul * 1.1f);
if (rand.NextDouble() < 0.05)
{
WanderRangeMul = Math.Min(1, WanderRangeMul * 1.5f);
}
}
float wRangeMul = WanderRangeMul;
double dx, dy, dz;
if (rand.NextDouble() < 0.05)
{
wRangeMul *= 3;
}
while (tries-- > 0)
{
dx = wanderRangeHorizontal.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
dy = wanderRangeVertical.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
dz = wanderRangeHorizontal.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
curTarget.X = entity.ServerPos.X + dx;
curTarget.Y = entity.ServerPos.Y + dy;
curTarget.Z = entity.ServerPos.Z + dz;
curTarget.W = 1;
Block block;
block = entity.World.BlockAccessor.GetBlock((int)curTarget.X, (int)curTarget.Y, (int)curTarget.Z);
if (!block.IsLiquid())
{
curTarget.W = 0;
}
else
{
curTarget.W = 1 / (Math.Abs(dy) + 1); //prefer not too much vertical change when underwater
}
//TODO: reject (or de-weight) targets not in direct line of sight (avoiding terrain)
if (preferredLightLevel != null && curTarget.W != 0)
{
tmpPos.Set((int)curTarget.X, (int)curTarget.Y, (int)curTarget.Z);
int lightdiff = Math.Abs((int)preferredLightLevel - entity.World.BlockAccessor.GetLightLevel(tmpPos, EnumLightLevelType.MaxLight));
curTarget.W /= Math.Max(1, lightdiff);
}
if (bestTarget == null || curTarget.W > bestTarget.W)
{
bestTarget = new Vec4d(curTarget.X, curTarget.Y, curTarget.Z, curTarget.W);
if (curTarget.W >= 1.0)
{
break; //have a good enough target, no need for further tries
}
}
}
if (bestTarget.W > 0)
{
FailedConsecutivePathfinds = Math.Max(FailedConsecutivePathfinds - 3, 0);
return(bestTarget.XYZ);
}
FailedConsecutivePathfinds++;
return(null);
}
public static extern ExceptionStatus core_FileStorage_shift_Vec4d(IntPtr fs, Vec4d val);
private Vec3d nearbyWaterOrRandomTarget()
{
int tries = 9;
Vec4d bestTarget = null;
Vec4d curTarget = new Vec4d();
BlockPos tmpPos = new BlockPos();
if (FailedConsecutivePathfinds > 10)
{
WanderRangeMul = Math.Max(0.1f, WanderRangeMul * 0.9f);
}
else
{
WanderRangeMul = Math.Min(1, WanderRangeMul * 1.1f);
if (rand.NextDouble() < 0.05)
{
WanderRangeMul = Math.Min(1, WanderRangeMul * 1.5f);
}
}
float wRangeMul = WanderRangeMul;
double dx, dy, dz;
if (rand.NextDouble() < 0.05)
{
wRangeMul *= 3;
}
while (tries-- > 0)
{
dx = wanderRangeHorizontal.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
dy = wanderRangeVertical.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
dz = wanderRangeHorizontal.nextFloat() * (rand.Next(2) * 2 - 1) * wRangeMul;
curTarget.X = entity.ServerPos.X + dx;
curTarget.Y = entity.ServerPos.Y + dy;
curTarget.Z = entity.ServerPos.Z + dz;
curTarget.W = 1;
Block block;
block = entity.World.BlockAccessor.GetBlock((int)curTarget.X, (int)curTarget.Y, (int)curTarget.Z);
if (!block.IsLiquid())
{
curTarget.W = 0;
}
else
{
curTarget.W = 1 / Math.Sqrt((dx - 1.0) * (dx - 1.0) + (dz - 1.0) * (dz - 1.0) + 1); //prefer target approx 1 block away
}
//TODO: reject (or de-weight) targets not in direct line of sight (avoiding terrain)
if (bestTarget == null || curTarget.W > bestTarget.W)
{
bestTarget = new Vec4d(curTarget.X, curTarget.Y, curTarget.Z, curTarget.W);
if (curTarget.W >= 1.0)
{
break; //have a good enough target, no need for further tries
}
}
}
if (bestTarget.W > 0)
{
FailedConsecutivePathfinds = Math.Max(FailedConsecutivePathfinds - 3, 0);
return(bestTarget.XYZ);
}
FailedConsecutivePathfinds++;
return(null);
}
public static void ConvertAnimation(string refpose, string input, string output)
{
NumberFormatInfo format = new NumberFormatInfo();
format.NumberDecimalSeparator = ".";
StreamReader refpose_reader = new StreamReader(refpose);
int refpose_bonecount = Convert.ToInt32(refpose_reader.ReadLine());
Dictionary <int, int> refpose_parentmap = new Dictionary <int, int>();
Dictionary <int, int> refpose_indexmap = new Dictionary <int, int>();
int[] refpose_bonearray = new int[refpose_bonecount];
int[] refpose_hierarchy = new int[refpose_bonecount];
refpose_reader.ReadLine();
refpose_reader.ReadLine();
for (int index = 0; index < refpose_bonecount; ++index)
{
string[] array = refpose_reader.ReadLine().Replace("\"", string.Empty).Split(' ');
refpose_bonearray[index] = Convert.ToInt32(array[1].Split('_').Last(), 16);
refpose_hierarchy[index] = Convert.ToInt32(array[2]);
}
for (int index = 0; index < refpose_bonecount; ++index)
{
if (refpose_hierarchy[index] != -1)
{
refpose_parentmap.Add(refpose_bonearray[index], refpose_bonearray[refpose_hierarchy[index]]);
}
else
{
refpose_parentmap.Add(refpose_bonearray[index], -1);
}
refpose_indexmap.Add(refpose_bonearray[index], index);
}
refpose_reader.ReadLine();
refpose_reader.ReadLine();
refpose_reader.ReadLine();
Vector3D[] refpose_position = new Vector3D[refpose_bonecount];
Vector3D[] refpose_rotation = new Vector3D[refpose_bonecount];
Vector3D[] refpose_scale = new Vector3D[refpose_bonecount];
for (int index = 0; index < refpose_bonecount; ++index)
{
refpose_position[index] = new Vector3D();
refpose_rotation[index] = new Vector3D();
refpose_scale[index] = new Vector3D(1, 1, 1);
string[] array = refpose_reader.ReadLine().Replace("\"", string.Empty).Split(' ');
refpose_position[index].X = Convert.ToSingle(array[2], format);
refpose_position[index].Y = Convert.ToSingle(array[3], format);
refpose_position[index].Z = Convert.ToSingle(array[4], format);
refpose_rotation[index].X = Convert.ToSingle(array[6], format);
refpose_rotation[index].Y = Convert.ToSingle(array[7], format);
refpose_rotation[index].Z = Convert.ToSingle(array[8], format);
}
refpose_reader.Close();
FileStream inputStream = new FileStream(input, FileMode.Open);
BinaryReader input_reader = new BinaryReader(inputStream);
input_reader.ReadInt32();
float duration = input_reader.ReadSingle();
float fps = input_reader.ReadSingle();
ushort bone_count = input_reader.ReadUInt16();
input_reader.ReadUInt16();
int frame_count = (int)(fps * (double)duration) + 1;
inputStream.Seek(24L, SeekOrigin.Current);
long offset_bone_list = input_reader.ReadInt64();
long offset_info_table = input_reader.ReadInt64();
inputStream.Seek(24L, SeekOrigin.Current);
StreamWriter output_writer = new StreamWriter(output);
output_writer.WriteLine("version 1");
output_writer.WriteLine("nodes");
int[] bone_list = new int[bone_count];
inputStream.Seek(offset_bone_list, SeekOrigin.Begin);
Dictionary <int, int> bone_translation_map = new Dictionary <int, int>();
int bone_id;
for (int index = 0; index < bone_count; ++index)
{
bone_id = input_reader.ReadInt32();
bone_list[index] = bone_id;
bone_translation_map.Add(bone_id, index);
}
for (int index = 0; index < bone_count; ++index)
{
bone_id = bone_list[index];
int num3 = -1;
if (refpose_parentmap.ContainsKey(bone_id))
{
int key2 = refpose_parentmap[bone_id];
num3 = !bone_translation_map.ContainsKey(key2) ? -1 : bone_translation_map[key2];
}
output_writer.WriteLine(index.ToString() + " \"bone_" + bone_id.ToString("X4") + "\" " + num3);
}
int last_bone_index = bone_count;
Dictionary <int, int> secondary_bone_translation_map = new Dictionary <int, int>();
for (int index = 0; index < refpose_bonecount; ++index)
{
if (!bone_translation_map.ContainsKey(refpose_bonearray[index]))
{
secondary_bone_translation_map.Add(refpose_bonearray[index], last_bone_index);
}
}
for (int index = 0; index < refpose_bonecount; ++index)
{
if (!bone_translation_map.ContainsKey(refpose_bonearray[index]))
{
int key2 = refpose_parentmap[refpose_bonearray[index]];
int num3 = !bone_translation_map.ContainsKey(key2) ? (!secondary_bone_translation_map.ContainsKey(key2) ? -1 : secondary_bone_translation_map[key2]) : bone_translation_map[key2];
output_writer.WriteLine(last_bone_index.ToString() + " \"bone_" + refpose_bonearray[index].ToString("X4") + "\" " + num3);
++last_bone_index;
}
}
output_writer.WriteLine("end");
float[,] x_array = new float[last_bone_index, frame_count];
float[,] y_array = new float[last_bone_index, frame_count];
float[,] z_array = new float[last_bone_index, frame_count];
float[,] sx_array = new float[last_bone_index, frame_count];
float[,] sy_array = new float[last_bone_index, frame_count];
float[,] sz_array = new float[last_bone_index, frame_count];
float[,] rx_array = new float[last_bone_index, frame_count];
float[,] ry_array = new float[last_bone_index, frame_count];
float[,] rz_array = new float[last_bone_index, frame_count];
float[,] rw_array = new float[last_bone_index, frame_count];
bool[,] has_rotation_frame = new bool[last_bone_index, frame_count];
bool[,] has_position_frame = new bool[last_bone_index, frame_count];
bool[,] has_scale_frame = new bool[last_bone_index, frame_count];
output_writer.WriteLine("skeleton");
output_writer.WriteLine("time 0");
for (int index = 0; index < bone_count; ++index)
{
if (refpose_indexmap.ContainsKey(bone_list[index]))
{
bone_id = refpose_indexmap[bone_list[index]];
output_writer.Write(index);
output_writer.Write(" " + refpose_position[bone_id].X.ToString("0.000000", format));
output_writer.Write(" " + refpose_position[bone_id].Y.ToString("0.000000", format));
output_writer.Write(" " + refpose_position[bone_id].Z.ToString("0.000000", format));
output_writer.Write(" " + refpose_rotation[bone_id].X.ToString("0.000000", format));
output_writer.Write(" " + refpose_rotation[bone_id].Y.ToString("0.000000", format));
output_writer.Write(" " + refpose_rotation[bone_id].Z.ToString("0.000000", format));
output_writer.WriteLine();
}
else
{
output_writer.WriteLine(index.ToString() + " 0 0 0 0 0 0");
}
}
int num4 = bone_count;
for (int index = 0; index < refpose_bonecount; ++index)
{
if (!bone_translation_map.ContainsKey(refpose_bonearray[index]))
{
output_writer.Write(num4++);
output_writer.Write(" " + refpose_position[index].X.ToString("0.000000", format));
output_writer.Write(" " + refpose_position[index].Y.ToString("0.000000", format));
output_writer.Write(" " + refpose_position[index].Z.ToString("0.000000", format));
output_writer.Write(" " + refpose_rotation[index].X.ToString("0.000000", format));
output_writer.Write(" " + refpose_rotation[index].Y.ToString("0.000000", format));
output_writer.Write(" " + refpose_rotation[index].Z.ToString("0.000000", format));
output_writer.WriteLine();
}
}
Quaternion3D q = new Quaternion3D();
Vector3D vector3D = new Vector3D();
inputStream.Seek(offset_info_table, SeekOrigin.Begin);
for (int index1 = 0; index1 < bone_count; ++index1)
{
long position = inputStream.Position;
int scale_count = input_reader.ReadInt16();
int position_count = input_reader.ReadInt16();
int rotation_count = input_reader.ReadInt16();
int flags = input_reader.ReadInt16();
long scale_indices_offset = input_reader.ReadInt32() * 4 + position;
long position_indices_offset = input_reader.ReadInt32() * 4 + position;
long rotation_indices_offset = input_reader.ReadInt32() * 4 + position;
long scale_data_offset = input_reader.ReadInt32() * 4 + position;
long position_data_offset = input_reader.ReadInt32() * 4 + position;
long rotation_data_offset = input_reader.ReadInt32() * 4 + position;
int[] scale_indices = new int[scale_count];
inputStream.Seek(scale_indices_offset, SeekOrigin.Begin);
for (int index2 = 0; index2 < scale_count; ++index2)
{
scale_indices[index2] = frame_count > byte.MaxValue ? input_reader.ReadInt16() : input_reader.ReadByte();
}
inputStream.Seek(scale_data_offset, SeekOrigin.Begin);
for (int index2 = 0; index2 < scale_count; ++index2)
{
int index3 = scale_indices[index2];
has_scale_frame[index1, index3] = true;
float x = input_reader.ReadUInt16() / 1024.0f;
sx_array[index1, index3] = x;
float y = input_reader.ReadUInt16() / 1024.0f;
sy_array[index1, index3] = y;
float z = input_reader.ReadUInt16() / 1024.0f;
sz_array[index1, index3] = z;
}
int[] position_indices = new int[position_count];
inputStream.Seek(position_indices_offset, SeekOrigin.Begin);
for (int index2 = 0; index2 < position_count; ++index2)
{
position_indices[index2] = frame_count > byte.MaxValue ? input_reader.ReadInt16() : input_reader.ReadByte();
}
inputStream.Seek(position_data_offset, SeekOrigin.Begin);
for (int index2 = 0; index2 < position_count; ++index2)
{
int index3 = position_indices[index2];
has_position_frame[index1, index3] = true;
float x = input_reader.ReadSingle();
x_array[index1, index3] = x;
float y = input_reader.ReadSingle();
y_array[index1, index3] = y;
float z = input_reader.ReadSingle();
z_array[index1, index3] = z;
}
int[] rotation_indices = new int[rotation_count];
inputStream.Seek(rotation_indices_offset, SeekOrigin.Begin);
for (int index2 = 0; index2 < rotation_count; ++index2)
{
rotation_indices[index2] = frame_count > byte.MaxValue ? input_reader.ReadInt16() : input_reader.ReadByte();
}
inputStream.Seek(rotation_data_offset, SeekOrigin.Begin);
for (int index2 = 0; index2 < rotation_count; ++index2)
{
ushort rot_a = input_reader.ReadUInt16();
ushort rot_b = input_reader.ReadUInt16();
ushort rot_c = input_reader.ReadUInt16();
Vec4d rot = Animation.UnpackRotation(rot_a, rot_b, rot_c);
int index3 = rotation_indices[index2];
has_rotation_frame[index1, index3] = true;
rx_array[index1, index3] = (float)rot.x;
ry_array[index1, index3] = (float)rot.y;
rz_array[index1, index3] = (float)rot.z;
rw_array[index1, index3] = (float)rot.w;
}
inputStream.Seek(position + 32L, SeekOrigin.Begin);
}
for (int index1 = 0; index1 < frame_count; ++index1)
{
output_writer.WriteLine($"time {index1 + 1}");
for (int index2 = 0; index2 < last_bone_index; ++index2)
{
if (has_position_frame[index2, index1] || has_rotation_frame[index2, index1] || has_scale_frame[index2, index1])
{
if (!has_position_frame[index2, index1])
{
int index3 = index1;
int index4 = index1;
while (!has_position_frame[index2, index3])
{
--index3;
}
while (index4 < frame_count && !has_position_frame[index2, index4])
{
++index4;
}
if (index4 == frame_count)
{
x_array[index2, index1] = x_array[index2, index3];
y_array[index2, index1] = y_array[index2, index3];
z_array[index2, index1] = z_array[index2, index3];
}
else
{
float num3 = (index1 - index3) / (float)(index4 - index3);
x_array[index2, index1] = (x_array[index2, index4] - x_array[index2, index3]) * num3 + x_array[index2, index3];
y_array[index2, index1] = (y_array[index2, index4] - y_array[index2, index3]) * num3 + y_array[index2, index3];
z_array[index2, index1] = (z_array[index2, index4] - z_array[index2, index3]) * num3 + z_array[index2, index3];
}
}
if (!has_scale_frame[index2, index1])
{
int index3 = index1;
int index4 = index1;
while (!has_scale_frame[index2, index3])
{
--index3;
}
while (index4 < frame_count && !has_scale_frame[index2, index4])
{
++index4;
}
if (index4 == frame_count)
{
sx_array[index2, index1] = sx_array[index2, index3];
sy_array[index2, index1] = sy_array[index2, index3];
sz_array[index2, index1] = sz_array[index2, index3];
}
else
{
float num3 = (index1 - index3) / (float)(index4 - index3);
sx_array[index2, index1] = (sx_array[index2, index4] - sx_array[index2, index3]) * num3 + sx_array[index2, index3];
sy_array[index2, index1] = (sy_array[index2, index4] - sy_array[index2, index3]) * num3 + sy_array[index2, index3];
sz_array[index2, index1] = (sz_array[index2, index4] - sz_array[index2, index3]) * num3 + sz_array[index2, index3];
}
}
if (!has_rotation_frame[index2, index1])
{
int index3 = index1;
int index4 = index1;
while (!has_rotation_frame[index2, index3])
{
--index3;
}
while (index4 < frame_count && !has_rotation_frame[index2, index4])
{
++index4;
}
if (index4 == frame_count)
{
rx_array[index2, index1] = rx_array[index2, index3];
ry_array[index2, index1] = ry_array[index2, index3];
rz_array[index2, index1] = rz_array[index2, index3];
rw_array[index2, index1] = rw_array[index2, index3];
}
else
{
double num3 = rx_array[index2, index4] * (double)rx_array[index2, index3] + ry_array[index2, index4] * (double)ry_array[index2, index3] + rz_array[index2, index4] * (double)rz_array[index2, index3] + rw_array[index2, index4] * (double)rw_array[index2, index3];
float num9 = (index1 - index3) / (float)(index4 - index3);
if (num3 < 0.0)
{
rx_array[index2, index1] = (-rx_array[index2, index4] - rx_array[index2, index3]) * num9 + rx_array[index2, index3];
ry_array[index2, index1] = (-ry_array[index2, index4] - ry_array[index2, index3]) * num9 + ry_array[index2, index3];
rz_array[index2, index1] = (-rz_array[index2, index4] - rz_array[index2, index3]) * num9 + rz_array[index2, index3];
rw_array[index2, index1] = (-rw_array[index2, index4] - rw_array[index2, index3]) * num9 + rw_array[index2, index3];
}
else
{
rx_array[index2, index1] = (rx_array[index2, index4] - rx_array[index2, index3]) * num9 + rx_array[index2, index3];
ry_array[index2, index1] = (ry_array[index2, index4] - ry_array[index2, index3]) * num9 + ry_array[index2, index3];
rz_array[index2, index1] = (rz_array[index2, index4] - rz_array[index2, index3]) * num9 + rz_array[index2, index3];
rw_array[index2, index1] = (rw_array[index2, index4] - rw_array[index2, index3]) * num9 + rw_array[index2, index3];
}
}
}
output_writer.Write(index2);
output_writer.Write(" " + x_array[index2, index1].ToString("0.000000", format));
output_writer.Write(" " + y_array[index2, index1].ToString("0.000000", format));
output_writer.Write(" " + z_array[index2, index1].ToString("0.000000", format));
q.i = rx_array[index2, index1];
q.j = ry_array[index2, index1];
q.k = rz_array[index2, index1];
q.real = rw_array[index2, index1];
Vector3D eulerAngles = C3D.ToEulerAngles(q);
output_writer.Write(" " + eulerAngles.X.ToString("0.000000", format));
output_writer.Write(" " + eulerAngles.Y.ToString("0.000000", format));
output_writer.Write(" " + eulerAngles.Z.ToString("0.000000", format));
output_writer.WriteLine();
}
}
}
output_writer.WriteLine("end");
output_writer.Close();
}
