Open Dynamics Engine 入門
【８日目】デモ「バスケットボール」

物理シミュレータ ODE（Open Dynamics Engine）の入門編の８日目です。 前回に引き続きポリゴンの表示のさせ方を勉強します。

【これまでの履歴】
■基本形１：【1日目】球の描画と衝突判定
■基本形２：【2日目】オブジェクトのジョイント
■デモ１：【3日目】デモ「カードタワー」
■デモ２：【4日目】デモ「いもむし」
■デモ３：【5日目】デモ「こま」
■デモ４：【6日目】デモ「円柱と球」
■デモ５：【7日目】デモ「落下するウサギ」

デモ「バスケットボール」

衝突計算用ジオメトリで球のコースを、三角メッシュデータで作っています。

プログラムソース

以下のプログラムソースは、C:/ode-0.11.1/ode\demo/demo_basket.cpp をもとにしております。

#include <assert.h>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
int WindowWidth  = 480; //ウィンドウの幅
int WindowHeight = 320; //ウィンドウの高さ

#ifndef DRAWSTUFF_TEXTURE_PATH
#define DRAWSTUFF_TEXTURE_PATH "C:/ode-0.11.1/drawstuff/textures"
#endif

static float world_vertices[] = {//頂点座標
-4,-4,-0.100000f,4,-4,-0.100000f,4,-4,0.100000f,-4,-4,-0.100000f,4,-4,0.100000f,-4,-4,0.100000f,4,0,0.100000f,4,-4,-0.100000f,4,4,-0.100000f,4,0,0.100000f,4,4,-0.100000f,4,4,0.100000f,4,0,0.100000f,4,-4,0.100000f,4,-4,-0.100000f,-4,-4,-0.100000f,-4,4,-0.100000f,4,4,-0.100000f,-4,-4,-0.100000f,4,4,-0.100000f,4,-4,-0.100000f,0.066000f,-2.060000f,2,0.066000f,-1.940000f,2,-0.066000f,-2.060000f,2,0.066000f,-1.940000f,2,-0.066000f,-1.940000f,2,-0.066000f,-2.060000f,2,-4,4,0.100000f,4,4,0.100000f,4,4,-0.100000f,4,4,-0.100000f,-4,4,-0.100000f,-4,4,0.100000f,-4,-4,0.100000f,-4,0,0.100000f,-4,-4,-0.100000f,-4,0,0.100000f,-4,4,0.100000f,-4,4,-0.100000f,-4,0,0.100000f,-4,4,-0.100000f,-4,-4,-0.100000f,0.360000f,3.244444f,1.466974f,0.360000f,3.422222f,2.266974f,-0.360000f,3.422222f,2.266974f,-0.360000f,3.422222f,2.266974f,-0.360000f,3.244444f,1.466974f,0.360000f,3.244444f,1.466974f,4,-4,0.100000f,0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-4,-4,0.100000f,4,-4,0.100000f,4,0,0.100000f,0.066000f,-1.940000f,0.100000f,4,-4,0.100000f,0.066000f,-1.940000f,0.100000f,0.066000f,-2.060000f,0.100000f,4,-4,0.100000f,-0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,0.100000f,4,0,0.100000f,4,0,0.100000f,-4,0,0.100000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-1.940000f,0.100000f,-4,0,0.100000f,-4,0,0.100000f,-4,-4,0.100000f,-0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,2,-0.066000f,-2.060000f,2,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,0.100000f,0.066000f,-2.060000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,2,0.066000f,-2.060000f,2,0.066000f,-2.060000f,2,0.066000f,-2.060000f,0.100000f,0.066000f,-1.940000f,1.950000f,0.066000f,-2.060000f,0.100000f,0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,1.950000f,-0.052853f,-1.506390f,2,0.052853f,-1.506390f,2,0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,2,-0.066000f,-2.060000f,0.100000f,-0.066000f,-2.060000f,2,-0.066000f,-1.940000f,1.950000f,-0.066000f,-2.060000f,0.100000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-2.060000f,2,-0.066000f,-1.940000f,2,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,-0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,0.100000f,0.066000f,-1.940000f,1.950000f,0.066000f,-1.940000f,0.100000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,2,-0.066000f,-1.840000f,2,-0.066000f,-1.840000f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.066000f,-1.940000f,1.950000f,-0.066000f,-1.940000f,2,0.066000f,-1.940000f,2,0.066000f,-1.840000f,2,-0.066000f,-1.940000f,2,0.066000f,-1.840000f,2,-0.066000f,-1.840000f,2,-0.066000f,-1.940000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,2,0.066000f,-1.940000f,1.950000f,0.066000f,-1.840000f,2,0.066000f,-1.940000f,2,-0.066000f,-1.840000f,2,-0.171600f,-1.740000f,2,-0.066000f,-1.840000f,1.950000f,-0.171600f,-1.740000f,2,-0.171600f,-1.740000f,1.950000f,-0.066000f,-1.840000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.171600f,-1.740000f,2,0.066000f,-1.840000f,1.950000f,0.171600f,-1.740000f,2,0.066000f,-1.840000f,2,-0.171600f,-1.740000f,2,-0.188760f,-1.640000f,2,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.171600f,-1.740000f,2,0.171600f,-1.740000f,1.950000f,0.188760f,-1.640000f,1.950000f,0.188760f,-1.640000f,2,0.171600f,-1.740000f,1.950000f,0.188760f,-1.640000f,2,0.171600f,-1.740000f,2,-0.188760f,-1.640000f,2,-0.132132f,-1.540000f,2,-0.132132f,-1.540000f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,2,0.188760f,-1.640000f,1.950000f,0.132132f,-1.540000f,1.950000f,0.132132f,-1.540000f,2,0.188760f,-1.640000f,1.950000f,0.132132f,-1.540000f,2,0.188760f,-1.640000f,2,-0.132132f,-1.540000f,2,-0.052853f,-1.506390f,2,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.132132f,-1.540000f,2,0.132132f,-1.540000f,1.950000f,0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,2,0.132132f,-1.540000f,1.950000f,0.052853f,-1.506390f,2,0.132132f,-1.540000f,2,0.188760f,-1.640000f,1.950000f,0.173397f,-1.642679f,1.950000f,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,1.950000f,0.132132f,-1.540000f,1.950000f,0.188760f,-1.640000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.157950f,-1.732697f,1.950000f,0.173397f,-1.642679f,1.950000f,0.171600f,-1.740000f,1.950000f,0.173397f,-1.642679f,1.950000f,0.188760f,-1.640000f,1.950000f,0.171600f,-1.740000f,1.950000f,0.066000f,-1.840000f,1.950000f,0.060149f,-1.825311f,1.950000f,0.171600f,-1.740000f,1.950000f,0.060149f,-1.825311f,1.950000f,0.157950f,-1.732697f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.060149f,-1.825311f,1.950000f,0.066000f,-1.840000f,1.950000f,-0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,1.950000f,0.066000f,-1.840000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.066000f,-1.840000f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.171600f,-1.740000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.188760f,-1.640000f,1.950000f,-0.132132f,-1.540000f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.132132f,-1.540000f,1.950000f,0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.052853f,-1.506390f,1.950000f,-0.052853f,-1.506390f,1.950000f,0.052853f,-1.506390f,1.950000f,0.049868f,-1.521079f,1.950000f,0.052853f,-1.506390f,1.950000f,0.132132f,-1.540000f,1.950000f,0.121808f,-1.551577f,1.950000f,0.052853f,-1.506390f,1.950000f,0.121808f,-1.551577f,1.950000f,0.049868f,-1.521079f,1.950000f,-0.188760f,-1.640000f,2,-0.173397f,-1.642679f,2,-0.121808f,-1.551577f,2,-0.121808f,-1.551577f,2,-0.132132f,-1.540000f,2,-0.188760f,-1.640000f,2,-0.171600f,-1.740000f,2,-0.157950f,-1.732697f,2,-0.173397f,-1.642679f,2,-0.173397f,-1.642679f,2,-0.188760f,-1.640000f,2,-0.171600f,-1.740000f,2,-0.066000f,-1.840000f,2,-0.060149f,-1.825311f,2,-0.171600f,-1.740000f,2,-0.060149f,-1.825311f,2,-0.157950f,-1.732697f,2,-0.171600f,-1.740000f,2,0.066000f,-1.840000f,2,0.060149f,-1.825311f,2,-0.066000f,-1.840000f,2,0.060149f,-1.825311f,2,-0.060149f,-1.825311f,2,-0.066000f,-1.840000f,2,0.171600f,-1.740000f,2,0.157950f,-1.732697f,2,0.060149f,-1.825311f,2,0.171600f,-1.740000f,2,0.060149f,-1.825311f,2,0.066000f,-1.840000f,2,0.173397f,-1.642679f,2,0.157950f,-1.732697f,2,0.171600f,-1.740000f,2,0.171600f,-1.740000f,2,0.188760f,-1.640000f,2,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.173397f,-1.642679f,2,0.188760f,-1.640000f,2,0.188760f,-1.640000f,2,0.132132f,-1.540000f,2,0.121808f,-1.551577f,2,0.052853f,-1.506390f,2,0.049868f,-1.521079f,2,0.121808f,-1.551577f,2,0.052853f,-1.506390f,2,0.121808f,-1.551577f,2,0.132132f,-1.540000f,2,-0.049868f,-1.521079f,2,0.049868f,-1.521079f,2,0.052853f,-1.506390f,2,0.052853f,-1.506390f,2,-0.052853f,-1.506390f,2,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.049868f,-1.521079f,2,-0.052853f,-1.506390f,2,-0.052853f,-1.506390f,2,-0.132132f,-1.540000f,2,-0.121808f,-1.551577f,2,-0.173397f,-1.642679f,2,-0.157950f,-1.732697f,2,-0.157950f,-1.732697f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.173397f,-1.642679f,2,-0.157950f,-1.732697f,2,-0.060149f,-1.825311f,2,-0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.157950f,-1.732697f,1.950000f,-0.157950f,-1.732697f,2,-0.060149f,-1.825311f,2,0.060149f,-1.825311f,2,0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,1.950000f,-0.060149f,-1.825311f,2,0.060149f,-1.825311f,1.950000f,0.060149f,-1.825311f,2,0.157950f,-1.732697f,2,0.157950f,-1.732697f,2,0.157950f,-1.732697f,1.950000f,0.060149f,-1.825311f,1.950000f,0.157950f,-1.732697f,2,0.173397f,-1.642679f,2,0.173397f,-1.642679f,1.950000f,0.173397f,-1.642679f,1.950000f,0.157950f,-1.732697f,1.950000f,0.157950f,-1.732697f,2,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,1.950000f,0.173397f,-1.642679f,1.950000f,0.173397f,-1.642679f,2,0.121808f,-1.551577f,2,0.049868f,-1.521079f,2,0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,1.950000f,0.121808f,-1.551577f,1.950000f,0.121808f,-1.551577f,2,0.049868f,-1.521079f,2,-0.049868f,-1.521079f,2,-0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,1.950000f,0.049868f,-1.521079f,2,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.121808f,-1.551577f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.049868f,-1.521079f,1.950000f,-0.049868f,-1.521079f,2,-0.121808f,-1.551577f,2,-0.173397f,-1.642679f,2,-0.173397f,-1.642679f,1.950000f,-0.173397f,-1.642679f,1.950000f,-0.121808f,-1.551577f,1.950000f,-0.121808f,-1.551577f,2,-0.360000f,3.600000f,0.100000f,0.360000f,3.600000f,0.100000f,4,4,0.100000f,4,4,0.100000f,-4,4,0.100000f,-0.360000f,3.600000f,0.100000f,-0.360000f,0.400000f,0.100000f,-0.360000f,3.600000f,0.100000f,-4,4,0.100000f,-4,4,0.100000f,-4,0,0.100000f,-0.360000f,0.400000f,0.100000f,4,0,0.100000f,0.360000f,0.400000f,0.100000f,-0.360000f,0.400000f,0.100000f,-0.360000f,0.400000f,0.100000f,-4,0,0.100000f,4,0,0.100000f,4,4,0.100000f,0.360000f,3.600000f,0.100000f,4,0,0.100000f,0.360000f,3.600000f,0.100000f,0.360000f,0.400000f,0.100000f,4,0,0.100000f,0.360000f,2.888889f,1.023752f,0.360000f,3.066667f,1.166974f,-0.360000f,3.066667f,1.166974f,-0.360000f,3.066667f,1.166974f,-0.360000f,2.888889f,1.023752f,0.360000f,2.888889f,1.023752f,0.360000f,2.533333f,0.939976f,0.360000f,2.711111f,0.966974f,-0.360000f,2.711111f,0.966974f,-0.360000f,2.711111f,0.966974f,-0.360000f,2.533333f,0.939976f,0.360000f,2.533333f,0.939976f,-0.360000f,2.177778f,0.939976f,0.360000f,2.177778f,0.939976f,0.360000f,2.355556f,0.939976f,0.360000f,2.355556f,0.939976f,-0.360000f,2.355556f,0.939976f,-0.360000f,2.177778f,0.939976f,-0.360000f,1.822222f,0.939976f,0.360000f,1.822222f,0.939976f,0.360000f,2,0.939976f,0.360000f,2,0.939976f,-0.360000f,2,0.939976f,-0.360000f,1.822222f,0.939976f,-0.360000f,1.466667f,0.939976f,0.360000f,1.466667f,0.939976f,0.360000f,1.644444f,0.939976f,0.360000f,1.644444f,0.939976f,-0.360000f,1.644444f,0.939976f,-0.360000f,1.466667f,0.939976f,0.360000f,1.111111f,0.957571f,0.360000f,1.288889f,0.939976f,-0.360000f,1.288889f,0.939976f,-0.360000f,1.288889f,0.939976f,-0.360000f,1.111111f,0.957571f,0.360000f,1.111111f,0.957571f,-0.360000f,0.755556f,1.134246f,0.360000f,0.755556f,1.134246f,0.360000f,0.933333f,1.009739f,0.360000f,0.933333f,1.009739f,-0.360000f,0.933333f,1.009739f,-0.360000f,0.755556f,1.134246f,0.360000f,0.755556f,1.134246f,-0.360000f,0.755556f,1.134246f,0.360000f,0.577778f,1.372130f,-0.360000f,0.755556f,1.134246f,-0.360000f,0.577778f,1.372130f,0.360000f,0.577778f,1.372130f,-0.360000f,3.600000f,3.900000f,-0.360000f,3.422222f,2.266974f,0.360000f,3.422222f,2.266974f,-0.360000f,3.600000f,3.900000f,0.360000f,3.422222f,2.266974f,0.360000f,3.600000f,3.900000f,0.360000f,3.244444f,1.466974f,-0.360000f,3.244444f,1.466974f,0.360000f,3.066667f,1.166974f,-0.360000f,3.244444f,1.466974f,-0.360000f,3.066667f,1.166974f,0.360000f,3.066667f,1.166974f,0.360000f,2.888889f,1.023752f,-0.360000f,2.888889f,1.023752f,0.360000f,2.711111f,0.966974f,-0.360000f,2.888889f,1.023752f,-0.360000f,2.711111f,0.966974f,0.360000f,2.711111f,0.966974f,0.360000f,2.533333f,0.939976f,-0.360000f,2.533333f,0.939976f,-0.360000f,2.355556f,0.939976f,0.360000f,2.533333f,0.939976f,-0.360000f,2.355556f,0.939976f,0.360000f,2.355556f,0.939976f,0.360000f,2.177778f,0.939976f,-0.360000f,2.177778f,0.939976f,-0.360000f,2,0.939976f,0.360000f,2.177778f,0.939976f,-0.360000f,2,0.939976f,0.360000f,2,0.939976f,0.360000f,1.822222f,0.939976f,-0.360000f,1.822222f,0.939976f,-0.360000f,1.644444f,0.939976f,0.360000f,1.822222f,0.939976f,-0.360000f,1.644444f,0.939976f,0.360000f,1.644444f,0.939976f,0.360000f,1.466667f,0.939976f,-0.360000f,1.466667f,0.939976f,-0.360000f,1.288889f,0.939976f,0.360000f,1.466667f,0.939976f,-0.360000f,1.288889f,0.939976f,0.360000f,1.288889f,0.939976f,0.360000f,1.111111f,0.957571f,-0.360000f,1.111111f,0.957571f,0.360000f,0.933333f,1.009739f,-0.360000f,1.111111f,0.957571f,-0.360000f,0.933333f,1.009739f,0.360000f,0.933333f,1.009739f,0.360000f,0.400000f,1.743932f,0.360000f,0.577778f,1.372130f,-0.360000f,0.577778f,1.372130f,-0.360000f,0.577778f,1.372130f,-0.360000f,0.400000f,1.743932f,0.360000f,0.400000f,1.743932f,
};
static dTriIndex world_indices[] = {
0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63,64,65,66,67,68,69,70,71,72,73,74,75,76,77,78,79,80,81,82,83,84,85,86,87,88,89,90,91,92,93,94,95,96,97,98,99,100,101,102,103,104,105,106,107,108,109,110,111,112,113,114,115,116,117,118,119,120,121,122,123,124,125,126,127,128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143,144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159,160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175,176,177,178,179,180,181,182,183,184,185,186,187,188,189,190,191,192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207,208,209,210,211,212,213,214,215,216,217,218,219,220,221,222,223,224,225,226,227,228,229,230,231,232,233,234,235,236,237,238,239,240,241,242,243,244,245,246,247,248,249,250,251,252,253,254,255,256,257,258,259,260,261,262,263,264,265,266,267,268,269,270,271,272,273,274,275,276,277,278,279,280,281,282,283,284,285,286,287,288,289,290,291,292,293,294,295,296,297,298,299,300,301,302,303,304,305,306,307,308,309,310,311,312,313,314,315,316,317,318,319,320,321,322,323,324,325,326,327,328,329,330,331,332,333,334,335,336,337,338,339,340,341,342,343,344,345,346,347,348,349,350,351,352,353,354,355,356,357,358,359,360,361,362,363,364,365,366,367,368,369,370,371,372,373,374,375,376,377,378,379,380,381,382,383,384,385,386,387,388,389,390,391,392,393,394,395,396,397,398,399,400,401,402,403,404,405,406,407,408,409,410,411,412,413,414,415,416,417,418,419,420,421,422,423,424,425,426,427,428,429,430,431,432,433,434,435,436,437,438,439,440,441,442,443,444,445,446,447,448,449,450,451,452,453,454,455,456,457,458,459,460,461,462,463,464,465,466,467,468,469,470,471,472,473,474,475,476,477,478,479,480,481,482,483,484,485,
};

// some constants

#define RADIUS 0.14

// dynamics and collision objects (chassis, 3 wheels, environment)

static dWorldID world;
static dSpaceID space;

static dBodyID sphbody; //球の動力学計算用ボディID
static dGeomID sphgeom; //球の衝突計算用ジオメトリID

static dJointGroupID contactgroup;
static dGeomID world_mesh;//メッシュの衝突計算用ジオメトリID

static void nearCallback (void *data, dGeomID o1, dGeomID o2)//衝突検出用コールバック関数
{
  assert(o1);
  assert(o2);

  if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
  {
    fprintf(stderr,"testing space %p %p\n", o1,o2);
    // colliding a space with something
    dSpaceCollide2(o1,o2,data,&nearCallback);
    // Note we do not want to test intersections within a space,
    // only between spaces.
    return;
  }

  //fprintf(stderr,"testing geoms %p %p\n", o1, o2);

  const int N = 32;
  dContact contact[N];
  int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
  if (n > 0) 
  {
    for (int i=0; i<n; i++) 
    {
      contact[i].surface.slip1 = 0.7;
      contact[i].surface.slip2 = 0.7;
      contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
      contact[i].surface.mu = 50.0; // was: dInfinity
      contact[i].surface.soft_erp = 0.96;
      contact[i].surface.soft_cfm = 0.04;
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,
		    dGeomGetBody(contact[i].geom.g1),
		    dGeomGetBody(contact[i].geom.g2));
    }
  }
}

// start simulation - set viewpoint
static void start(){
  //dAllocateODEDataForThread(dAllocateMaskAll); //スレッドの割り当てを行う。dInitODE2()関数で初期化をした際には必要なし
  static float xyz[3] = {-6,0,4};
  static float hpr[3] = {0.0f,-22.5000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
}

static void reset_ball(void){
  float sx=0.0f, sy=3.39f, sz=7.05;
  dQuaternion q;
  dQSetIdentity(q);
  dBodySetPosition (sphbody, sx, sy, sz);
  dBodySetQuaternion(sphbody, q);
  dBodySetLinearVel (sphbody, 0,0,0);
  dBodySetAngularVel (sphbody, 0,0,0);
}

// called when a key pressed
static void command (int cmd){
  switch (cmd) {
    case ' ':
	  reset_ball();
      break;
  }
}

// simulation loop
static void simLoop (int pause){
  double simstep = 0.001;     //シミュレーションの時間を設定「1ms」
  double dt = dsElapsedTime();//経過時間を測定する

  int nrofsteps = (int) ceilf(dt/simstep); //実時間とシミュレーションの時間の調整を行う
  //fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps);

  //シミュレーションの時間経過と実世界の時間経過を一致させる
  for (int i=0; i<nrofsteps && !pause; i++){ 
    dSpaceCollide (space,0,&nearCallback);
    dWorldQuickStep (world, simstep);
    dJointGroupEmpty (contactgroup);
  }
  dsSetColor (1,1,1);
  const dReal *SPos = dBodyGetPosition(sphbody);//球の位置を取得
  const dReal *SRot = dBodyGetRotation(sphbody);//球の回転角度を取得
  float spos[3] = {SPos[0], SPos[1], SPos[2]};  //
  float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
  dsDrawSphere(spos, srot, RADIUS);

  // draw world trimesh
  dsSetColor (1,0.5f,0);
  dsSetTexture (DS_WOOD);

  const dReal* Pos = dGeomGetPosition(world_mesh);
  //dIASSERT(dVALIDVEC3(Pos));
  float pos[3] = { Pos[0], Pos[1], Pos[2]};

  const dReal* Rot = dGeomGetRotation(world_mesh);
  //dIASSERT(dVALIDMAT3(Rot));
  float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11]};

  int numi = sizeof(world_indices) / sizeof(dTriIndex);

  for (int i=0; i<numi/3; i++){
    int i0 = world_indices[i*3+0];
    int i1 = world_indices[i*3+1];
    int i2 = world_indices[i*3+2];
    float *v0 = world_vertices+i0*3; 
    float *v1 = world_vertices+i1*3;
    float *v2 = world_vertices+i2*3;
    dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
  }
}
int main (int argc, char **argv)
{
  dMass m;
  dMatrix3 R;
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

  // create world
  dInitODE2(0);
  world = dWorldCreate();
  space = dHashSpaceCreate (0);

  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-9.8);
  dWorldSetQuickStepNumIterations (world, 64);//dWorldQuickStep関数利用時の計算精度の向上（デフォルト：20）

  // Create a static world using a triangle mesh that we can collide with.
  int numv = sizeof(world_vertices)/(3*sizeof(float));
  int numi = sizeof(world_indices)/ sizeof(dTriIndex);
  printf("numv=%d, numi=%d\n", numv, numi);
  dTriMeshDataID Data = dGeomTriMeshDataCreate(); //メッシュデータIDの取得

//  fprintf(stderr,"Building Single Precision Mesh\n");

  dGeomTriMeshDataBuildSingle//メッシュデータの代入
  (
    Data, 
    world_vertices, 
    3 * sizeof(float), 
    numv, 
    world_indices, 
    numi, 
    3 * sizeof(dTriIndex)
  );
  world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);//メッシュデータの生成し、ジオメトリにセットする
  dGeomTriMeshEnableTC(world_mesh, dSphereClass, false); // ?
  dGeomTriMeshEnableTC(world_mesh, dBoxClass, false);    // ?
  dGeomSetPosition(world_mesh, 0, 0, 0.5); //
  dRSetIdentity(R); //回転行列の初期化
  //dIASSERT(dVALIDMAT3(R));
  dGeomSetRotation (world_mesh, R);

  //球の設定
  sphbody = dBodyCreate (world); //
  dMassSetSphere (&m,1,RADIUS);
  dBodySetMass (sphbody,&m);
  sphgeom = dCreateSphere(0, RADIUS);
  dGeomSetBody (sphgeom,sphbody);
  reset_ball();
  dSpaceAdd (space, sphgeom);

  // run simulation
	dsSimulationLoop (argc,argv,WindowWidth,WindowHeight,&fn);

  // Causes segm violation? Why?
  // (because dWorldDestroy() destroys body connected to geom; must call first!)
  dGeomDestroy(sphgeom);
  dGeomDestroy (world_mesh);

  dJointGroupEmpty (contactgroup);
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}